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I.  INTRODUCTION 

A.  MOTIVATION 

There  is  an  increasing  demand  for  realistic  virtual  environments  (VE).  One  of  the 
main  branches  of  research  in  VE  is  human  motion  simulation.  Kinematics  and  dynamics 
are  the  two  disciplines  which  are  used  to  create  physically  based  mathematical  models  for 
human  motion  simulation.  Kinematic  models  are  quite  simple  to  implement  and 
computationally  inexpensive  in  comparison  to  dynamic  models.  However,  they  are  limited 
to  simulation  of  the  geometric  constraints  of  the  body  parts  of  the  human.  On  the  other 
hand,  dynamic  models  introduce  additional  physical  properties  of  the  body  parts  to  the 
simulation,  such  as  mass  and  moment  of  inertia,  which  provides  more  realism.  It  is  possible 
to  simulate  human  motion  realistically  with  detailed  dynamic  models.  However,  the  cost  of 
this  realism  is  a  high  degree  of  computational  complexity.  When  more  detailed  models  are 
chosen,  the  response  time  of  the  simulated  human  model  increases.  On  the  other  hand, 
reducing  latency  to  a  minimum  amount  of  time  is  a  important  requirement  for  virtual 
environments  in  which  humans  can  interact.  This  places  limits  on  the  complexity  of  the 
model  which  can  be  used  in  a  particular  situation. 

B.  GOALS 

The  purpose  of  this  thesis  is  to  develop  a  single  rigid  body  dynamic  human  model 
with  massless  legs  to  illustrate  that  such  a  model  can  simulate  human  motion  more 
smoothly  and  more  realistically  than  kinematic  models,  and  without  resorting  to  the 
complexity  of  multi-link  dynamic  models.  The  Newton-Euler  method  is  chosen  instead  of 
the  Lagrangian  method,  since  the  complexity  of  the  latter  grows  exponentially  when  the 
degrees  of  freedom  increase. 


C.     ORGANIZATION 

Chapter  II  of  this  thesis  provides  background  information  and  reviews  previous 
work  relating  to  the  area  of  kinematic  and  dynamic  methods  to  simulate  human  motion.  In 
this  chapter,  articulated  rigid  body  dynamics,  postural  control,  and  control  of  stepping  are 
also  investigated  in  addition  to  single  rigid  body  dynamics.  Chapter  III  provides  an 
overview  of  the  problem  statement  for  this  thesis  and  discusses  mathematical  modeling  of 
postural  control  of  a  single  rigid  body  with  an  attached  massless  leg.  Chapter  IV  introduces 
the  developed  kinematic  and  dynamic  computer  models.  Chapter  V  presents  results  and 
conclusions  about  the  work  completed.  The  last  chapter,  Chapter  VI,  discusses 
recommendations  for  future  enhancements  and  further  research. 


II.  SURVEY  OF  PREVIOUS  WORK 

A.  INTRODUCTION 

Mathematical  models  which  define  human  motion  in  detail  are  complex  and  quite 
nonlinear.  This  provides  a  motivation  to  start  with  simple  models  at  a  high  level  of 
abstraction.  Models  which  ignore  the  dynamic  properties  of  body  parts  are  called  kinematic 
models.  There  are  also  models  which  consider  only  the  torso  to  have  dynamic  properties. 
More  complex  models  introduce  mass  and  moment  of  inertia  for  more  than  one  body  part. 
The  more  dynamic  properties  are  added,  the  higher  becomes  the  complexity  and  non- 
linearity  of  the  resulting  model. 

B.  KINEMATIC  MODELS 

Kinematics  is  concerned  with  motion  without  considering  the  forces  and  torques 
that  cause  it.  It  establishes  relations  between  position,  velocity,  acceleration,  and  higher 
order  derivatives  of  position  variables. 

The  human  body  can  be  thought  as  a  set  of  connected  body  parts  (links).  Each  body 

part  (link)  has  four  link  parameters  to  be  defined:  link  length,  (a),  link  twist  (a),  link 
offset(d)  and,  joint  angle(9).  There  exists  a  coordinate  frame  attached  to  each  link 

[CRAI89]. 

Manipulator  kinematics  investigates  the  transformation  from  frame  to  frame  as  the 
body  articulates.  Forward  kinematics  show  how  to  compute  the  position  and  orientation  of 
the  end-effector  according  to  link  parameters.  On  the  other  hand,  inverse  kinematics  solves 
for  link  parameters  when  the  position  and  orientation  of  the  end-effector  is  given. 

For  the  human  body,  it  can  be  assumed  that  the  joint  angle  is  the  only  variable  link 
parameter.  Forward  kinematics  can  be  used  to  solve  for  the  position  and  orientation  of  each 
body  part  as  joint  angles  are  given.  However,  this  method  is  difficult  for  the  animator.  The 


position  and  orientation  of  each  body  part  can  also  be  computed  with  the  position  and 
orientation  of  the  end-effector  as  an  input  by  using  inverse  kinematics.  Even  though  the 
second  method  introduces  more  complexity,  in  this  approach  the  animator  is  able  to  define 
the  path  for  the  end-effector. 

C.      SINGLE  RIGID  BODY  DYNAMICS 

An  advanced  approach  to  human  motion  simulation  introduces  dynamics.  There  are 
three  most  often  used  methods:  free  body  method  with  hard  constraints,  free  body  method 
with  soft  constraints,  and  generalized  coordinates  with  Lagrangian[MCGH79]. 

1.    Free  Body  Method  with  Hard  Constraints 

This  method  looks  at  each  body  part  as  a  separate  free  element,  under  the  influences 
of  joint  torques,  gravity,  and  reaction  forces  and  moments.  The  simplest  possible  dynamical 
model  for  human  motion  is  an  inverted  pendulum  which  idealizes  the  entire  body  as  a 
single  rigid  link  with  upright  posture  maintained  by  ankle  torque. 
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Figure  1:  Inverted  Pendulum 


In  human  motion,  joint  torques  are  created  by  muscular  contraction.  The  origin  of 
reaction  forces  and  moments  is  forces  from  other  limbs  or  the  ground.  The  force  and  torque 
equations  for  the  inverted  pendulum  are  [MCGH79] 

mx  =  Fx  (eq.2.1) 

my  =  Fy-mg  (eq.2.2) 

76  =  F^/sinS-Fy/cosG  +  M  (eq.  2.3) 

where  /    is  the  distance  from  the  coordinate  origin  to  the  center  of  gravity,  I  is  moment  of 
inertia  about  the  center  of  gravity,  F   ,  F      are  ground  reaction  forces,  and  M    is  the  joint 

control  torque  which  is  computed  by  some  control  law 

M  =   u(x,  t).  (eq.  2.4) 

In  this  equation,  x  is  the  system  state  vector  and  t  is  time. 

After  expressing  reaction  forces  and  moments,  constraint  equations  are  derived 
according  to  the  geometry  of  the  system,  and  differentiated  twice.  Constraint  equations  for 
this  example  are 

x  =   /COS0  (eq.  2.5) 

and 

y  =  /sin 6  (eq.  2.6) 

with  second  derivatives 

x  =  -9/sine-e  /cosG  (eq.2.7) 


and 


y  =  e/cosG-G  /sinG  (eq.2.8) 


The  five  equations  above  can  be  solved  numerically  or  analytically  to  determine  the 


unknowns:  i:',  y,  0,  F   ,  F   .  The  matrix  form  of  the  equations  is 

x       v 


m  0        0 
0  m       0 

0  0        / 

1  0    /sinO 
0    1  -/cosG 


-1          0 

X 

0         -1 

y 

/sinO  /cosO 

e 

= 

0          0 

Fx 

0          0 

F 

y_ 

0 
-mg 

M 

-e  /cose 

-9  /sine 


(eq.  2.9) 


The  behavior  of  the  pendulum  can  also  be  expressed  with  a  3x3  matrix 
multiplication  by  analytically  eliminating  x  and  y.  The  3x3  matrix  equation  is 
[MCGH79b] 


/         -/sinO  /cosO 
ra/sinO         1  0 

-m/cosO       0  1 


Since  this  system  has  only  a  single  degree  of  freedom,  a  suitable  state  vector  is 


e 

M 

Fx 

= 

.  2 
-raO  /cosO 

Fz 

.2 
-m0  /sinO  +  mg 

(eq.2.10) 


X  = 


e^ 


(eq.  2.11) 


so 


x  = 


& 


\x2) 


(eq.2.12) 


For  simulating  this  model,  numerical  integration  can  be  used  to  compute  x(t)  for 


any  given  x(0) .  The  value  of  0  (or  x^ )  is  calculated  by  the  system  dynamic  equations.  It 


has  been  found  in  the  work  of  this  thesis  that  the  3x3  matrix  solution  is  approximately  two 
times  faster  than  the  5x5  one. 


2.    Free  Body  Method  with  Soft  Constraints 

A  more  realistic  approach  is  to  consider  the  connection  between  limb  segments  as 
not  rigid.  In  the  soft  constraint  method,  the  constraint  equations  are  replaced  with  functions 
defined  by  joint  variables  and  their  derivatives,  which  are  used  to  compute  constraint  forces 
with  some  gain  values.  This  increases  the  order  of  state  equation  by  a  factor  of  three  in  the 
planar  case,  and  by  a  factor  of  six  in  the  three  dimensional  case,  but  avoids  matrix 
inversion. 

The  previous  inverted  pendulum  model  becomes  as  shown  in  Figure  2  for  the  soft 
constraint  method. 
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Figure  2:  Inverted  Pendulum  with  Soft  Constrains 

Instead  of  Eq.  2.5  and  Eq.  2.6,  the  soft  constraint  equations  are[MCGH79] 
x  =  xb  +  /cos6 

y  =  y,  +  /sin  6 
with  the  corresponding  soft  constraint  forces 


(eq.2.13) 
(eq.  2.14) 


Fx  =  -kxXb~kx*b  (eq.2.15) 

Fy  =  ~kyyb~kyyb  (eq.2.16) 

The  state  vector  for  the  numerical  solution  is 

•     T 

x  =  (6,  9,  xb,  xb,  yb,  yb)  (eq.  2.17) 

After  obtaining  the  constraint  forces  by  using  the  state  vector  values,  Eq  2.1,  Eq  2.2,  and 
Eq  2.3  can  be  solved  directly  for  translational  and  angular  accelerations  instead  of  requiring 
matrix  inversion. 

3.    Generalized  Coordinates  with  Lagrangian 

Another  way  to  derive  the  system  equations  is  to  use  the  Lagrangian  method.  This 
method  does  not  compute  constraint  forces  and  moments,  but  instead  uses  the  difference  of 
kinetic  and  potential  energies  expressed  in  terms  of  generalized  coordinates. 

The  virtual  work  function,  for  any  generalized  coordinate,  6 ,  is 

hW  =   QeSQ  (eq.  2.18) 

The  Lagrangian  function  is 

L  =  K-V  (eq.  2.19) 

where  K  is  the  kinetic  energy  function  and  V  is  the  potential  energy  function.  The 
differential  equations  of  the  system  are  obtained  from  [MCGH79b] 

—  -^-=2  (eq.2.20) 

d!dqt     oqt  q< 

where  q  ■  represents  the  generalized  coordinate  for  each  i  and  Q      is  the  coefficient  of 

hq-  in  the  virtual  work  function. 


Considering  the  hard  constraint  example,  Figure  1 ,  the  virtual  work  function  is 

§W  =  M86  (eq.  2.21) 

where  6  is  the  generalized  coordinate.  The  kinetic  energy  of  this  system  is 

12  2-2 

T  =  -(mx    +my    +79   )  (eq.  2.22) 


or 


1        2-2  2-2  .2 

T  =  -(ml  6  sin6  +  ra/  6  cos0  +  /9  )  (eq.  2.23) 

which  simplifies  to 

1        2  -2 

T  =  -(ml    +7)6  (eq.  2.24) 

The  potential  energy  of  the  system  is 

V  =  rag/sin6  (eq.  2.25) 

Thus,  the  Lagrangian  function  is 

1         2  -2 

L  -  -(ml    +7)6    -rag/sin6  (eq.  2.26) 

By  evaluation  the  Lagrangian  derivatives,  the  result  is  that  the  angular  acceleration  of  the 
pendulum  is  given  by 

e  =  M-msiQQ^  (eq.  2.27) 

I  +  ml 
For  a  numerical  solution,  the  state  vector  is 

"ffl-Q 


The  Lagrangian  approach  is  perhaps  the  most  effective  one  for  simple  cases. 
However,  with  an  increase  in  the  number  of  degrees  of  freedom  of  the  system  the 
complexity  of  the  calculations  grows  exponentially. 

D.      ARTICULATED  RIGID  BODY  DYNAMICS 

It  is  certainly  impossible  to  model  the  human  body  with  complete  realism  as  a  single 
mass  rigid  body.  One  can,  however,  abstract  it  as  a  system  of  rigid  bodies  connected 
together  with  rotary  joints  with  control  torques  acting  at  each  joint.  If  the  motion  of  each 
limb  segment  is  given,  and  the  forces  and  torques  are  to  be  computed,  this  problem  is  called 
the  inverse  dynamics  problem.  If  the  torques  are  given  and  the  accelerations  are  to  be 
determined,  this  is  called  the  direct  dynamics  problem  [KOOZ83]. 

There  is  an  algorithm,  called  Articulated  Body(AB),  for  direct  dynamics  which 
contains  three  0(  n)  recursions  [MCMI95].  In  the  first  step  of  this  method,  which  is 
Forward  Kinematics,  velocity  and  velocity  dependent  terms  are  computed  from  the  base  to 
the  tip  of  each  serial  chain  of  links.  The  orientation  of  i  's  coordinate  system  with  respect 

to  /  —  1  's  specified  by  the  rotation  matrix  [CRAI89],  R-  _  j ,  can  be  determined  by  using 
joint  position,  q-,  which  is  an  element  of  state  vector.  The  position  of  i's  coordinate 

system  origin  according  to  i  —  1 ,         p. ,  is  a  constant.  Then  the  spatial  transformation 
matrix  is  [CRAI89] 

i  + 


i+%  = 


Rt  0 

Pl+lx        R.  R 


(eq.  2.29) 
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The  spatial  transformation  matrices,  along  with  qaie  used  to  compute  velocity 
values  of  the  links  by  using  the  relation 

vi  =  *X  i-  1  vi-\  («*  2-3°) 

The  velocity-dependent  bias  forces,    B-,  and  the  vector  of  Coriolis  and  centripetal 

accelerations,  C ,  of  each  link  are  also  determined  from  the  base  to  the  tip.  Details  of  this 
calculation  can  be  found  in  [MCMI95b]. 

In  the  second  step,  which  is  backwards  dynamics,  1^  ■  and  (3^  •  are  computed  from 

the  tip  to  the  base.  The  matrix  /t  •  is  the  6X6  inertia  of  links  i  though  N .  In  other  words, 
"the  inertia  'felt'  at  the  i  coordinate  system  when  the  joints  from  i  +  1  to  N  are  free  to 
move"[MCMI95].  The  vector  (3t-  is  the  bias  force  exerted  on  2th  link,  including  all 

outboard  torques. 

In  the  final  step,  joint  and  link  accelerations  are  computed  from  the  base  to  the  tip 

as  <2q  =  0  is  given  and  by  using  the  equations 

and 

/.  =  /t-a'.-pt.  (eq.2.32) 

where  /  •  is  the  spatial  force  exerted  onto  link  i  by  its  inboard  link  which  contains  the  effect 

of  input  torque,   T  ,  and  where  d    is  a  six  element  vector  containing  the  angular 
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acceleration,  CO-,  and  translational  acceleration  vectors.  Each  <j).  is  a  six  element  unit 

vector  which  specifies  the  corresponding  joint  axis. 

Another  numerical  approach  to  the  recursive  direct  dynamics  is  based  on  solving 
the  inverse  dynamics  problem  for  a  sequence  of  n  +  1  specialized  acceleration  vectors.  In 
general,  the  inverse  dynamic  equation  can  be  written  as  [KOOZ83] 

T  =   C6  -  D  (eq.  2.33) 

where  the  matrix  C  is  given  by 

C  =  B~lJ  =  [ClC2C3...]  (eq.2.34) 

and  where  each  element  of  vector  C  is  a  column  vector.  Usually,  C  is  not  known 
explicitly.  However  it  can  be  computed  with  a  numerical  approach  using  an  inverse 

T 

dynamics  model  such  as  that  in  [CRAI89].  Specifically,  for  the  case  9  =  (0,  0,  ...)    , 

from  Eq  2.33 

T0  =  -D  (eq.  2.35) 

The  torque  T ^  can  be  computed  by  using  the  kinematic  and  dynamic  equations  of  the 

articulated  mechanism.  As  an  example,  the  model  given  in  [KOOZ83]  which  is  illustrated 
in  Figure  3  can  be  used. 

The  kinematic  equations  of  the  planar  model  in  Figure  3  are 

.2 
xt  =  xi_l-(li_l-di_l)(Qi_lsmQi_l+Qi_icosQi_l) 

.2 
-di(QisinQi  +  6,-  cosGp  (eq.  2.36) 

and 
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y'i  =  y'i-\  +  (//_  i  -d{_  i)(8f_  icosez._  rQt_  x  sine.,^ 

.2 

+rf.(elcoserefcose/) 


;+ 1 


(eq.  2.37) 


Figure  3:  Free- body  for  Typical  Link  of  Serial  Open-chain  Planar  Articulated  Mechanism 

[KOOZ83] 

Eq.  2.36  and  Eq.  2.37  are  used  to  calculate  the  translational  accelerations  of  the 
links  from  the  base  to  the  tip.  By  using  these  accelerations,  joint  torques  can  be  computed 
in  the  same  recursive  manner,  but  from  out  to  inward  with  the  following  equations: 


F  „   =  F„      +  m-X; 


ii+i 


i  i 


Tt  =  Ti+rt'xtJlrdi>*a*t*FjlJh-di>a»*t 


-F '  d.sviQ.  +  F '  d.cosQ.-  +  J '.9.- 

^v.'       I  I  y :       I  I  LI 


(eq.  2.38) 
(eq.  2.39) 
(eq.  2.40) 
(eq.2.41) 
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T 

For  the  case  0  =  ( 1 ,  0,  . . . )    ,  all  accelerations  except  0  j  would  be  equal  to  zero 

which  means 

Tl    =   Cx-D  (eq.2.42) 

The  joint  torque,  T , ,  can  be  calculated  with  the  recursive  method,  which  is  described 

above.  Then  C\  can  be  calculated  as 

Cj    =   Tl+D  =   T{-TQ  (eq.2.43) 

With  the  same  logic  C  ■  is 

C;  =  Tt  -  TQ  (eq.  2.44) 

Thus,  the  C  matrix  can  be  computed  numerically  by  following  these  steps  n  times  for  a  n- 
link  system.  Then  0  is  given  by 

0  =   C~l(T-T0)  (eq.2.45) 

where  T  is  any  arbitrary  torque  and  Tq  is  the  "equilibrium"  torque  defined  by  Eq  2.35. 

3 
Because  of  the  required  matrix  inversion,  the  second  method  has  0(n    )  complexity.  It  has 

3 
been  shown  [MCMI94],  that  0(n    )  methods  are  best  for  simple  serial  chains  of  rigid 

bodies  when  n  <  3  ,  and  0(  n )  methods  are  better  for  n  >  3  . 

E.      POSTURAL  CONTROL 

It  is  clearly  necessary  to  begin  with  modeling  stable  standing  of  a  human  before 
considering  control  of  walking.  All  the  methods  derived  in  the  previous  sections  show  how 
to  compute  joint  angle  accelerations  according  to  joint  control  torques.  The  question  is  how 
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to  formulate  control  torques.  One  straightforward  way  is  linear  state  feedback  control 
[MCGH86]. 

A  suitable  state  vector  for  postural  control  can  be  defined  as 


x  = 


6 
9 


(eq.  2.46) 


where  0  is  an  nxl  vector  of  joint  positions  and  6  is  the  nxl  vector  of  joint  velocities.  The 
corresponding  body  state  equation  is[KOOZ83] 

i  =  f(x)+E(x)    T  (eq.2.47) 

In  Eq.  2.10  /  and,  E  are  in  general  not  available  analytically.  However,  with  the 
numerical  approach,  explained  in  the  previous  section,  the  same  equation  can  be  expressed 
as 


x  = 


0  / 
0  0 


x  + 


C 


0 
-1 


(T-TQ) 


(eq.  2.48) 


where  /  is  the  unit  matrix. 

Linear  feedback  systems  permit  much  flexibility  to  the  designer,  such  as  pole 

assignment  [KOOZ83].  Since  C  and  Tq  are  functions  of  X,  the  system  is  not  linear. 
However,  it  can  be  linearized  around  0  for  joint  angular  rates  and  90  degrees  for  joint  angles 
for  erect  body  position.  Then  the  linearized  body  state  equation  can  be  written  as 

x  =  Fx+GT  (eq.  2.49) 

where   F   is  the  linearized  system  matrix  and   G    is  the  control  distribution  matrix 

[KOOZ83]. 

Under  the  assumption  of  linear  state  feedback,  the  control  torques  vector  will  be 
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T  =  K  X  (eq.  2.50) 

where  K  is  the  gain  matrix.  If  Eq.  2.12  and  Eq.  2.13  are  combined,  the  derivative  of  the 
state  vector  is  then 

x  =  Fx  +  GKx  =  [F  +  GK]x  =  Hx  (eq.2.51) 

Gain  values  in  K  should  be  chosen  so  that  all  eigenvalues  of  H  have  negative  real  parts  in 
order  to  obtain  a  stable  postural  control  [KOOZ83]  [CAMA77]. 

F.      CONTROL  OF  STEPPING 

If  all  the  postural  control  system  eigenvalues  have  negative  real  parts,  the 
articulated  mechanism  can  maintain  its  upright  standing  posture.  The  next  step  is  to  add 
stepping  to  the  system.  At  this  point  there  are  two  options  to  choose:  static  or  dynamic 
balancing.  "A  statically  balanced  system  avoids  tipping  and  ensuing  horizontal 
accelerations  by  keeping  the  center  of  mass  of  the  body  over  the  polygon  of  the  support, 
formed  by  the  feet."  [RABI86].  On  the  other  hand,  a  dynamically  balanced  system  can 
depart  from  static  equilibrium  and  is  permitted  to  tip  and  accelerate  for  short  period  of  time. 
By  observing  human  walking,  one  can  easily  say  that  dynamical  balancing  needs  to  be 
chosen  for  simulating  a  stepping  human. 

A  step  length  control  method  with  dynamic  stability  was  presented  by  Gubina 
[GUBI74].  Gubina' s  model  has  a  single  rigid  body  with  two  supporting  massless  legs.  The 
system  has  three  degrees  of  freedom.  The  state  vector  for  the  linearized  system  is 

x  =  (r-r0,r,ei,e1,G2,e2)  (eq.2.52) 

where  9 ,   is  the  leg  angle,  92  is  the  body  attitude,    r    is  the  leg  length,  and  Tq  is  the 
desired  leg  length. 


16 


/ 

i 

K      /   M 

F  A 

r  / 

n  r  \\       "> 

7  A    / 

7    >    \  ' 

/      \i 

/          * 

Figure  4:  Single  Rigid  Body  Model  with  Variable  Length  Massless  Supporting  Legs 

[GUBI74] 


where 


and 


The  input  vector  for  the  linear  state  equations  is 

T 
U    =    (W|,  Uj) 


u\   =  F~F0 


(eq.  2.53) 


(eq.  2.54) 


u2  =  M-Mq 


(eq.  2.55) 


In  these  equations,  F  is  the  control  force  applied  along  the  leg,  M  is  the  control  torque 

applied  at  the  hip  and  FQ,  Mq  are  bias  force  and  torque  respectively. 

The  input  vector,  u ,  is  used  to  control  leg  length,  and  body  attitude  and  step  control 
are  used  to  produce  the  desired  forward  motion.  The  leg  length  is  controlled  by  u  j  as 
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Hj   =  hl(r-r0)  +  h2f  (eq.  2.56) 

where  h ,  and  /i~  are  gain  constants.  The  body  attitude  is  controlled  by  u^  as 

u2  =  h5(Q2-a)  +  h6Q2  (eq.  2.57) 

where  h<?  and  /i^  are  gain  constants  and  OC  is  the  bias  term  to  permit  the  desired  attitude. 

When  the  initial  conditions  are  assumed  as  r(0)  =  77),  r(0)  =  0, 
0o(O)  =  0 ,  62(^)  =  0 ,  the  leg  angle  is  governed  by  the  linearized  system  equation 


Qi-b  9j  =  0 


(eq.  2.58) 


where 


b2=    * 


Co"') 


(eq.  2.59) 
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Figure  5:  Stable  Gait  Function  [GUBI74} 

As  seen  in  Figure  5,  the  leg  has  a  negative  angular  velocity  growth  rate  for  negative 
angles  and  positive  angular  velocity  growth  rate  for  positive  angles.  The  horizontal  dotted 
line  which  is  extended  from  the  right  to  the  left  represents  the  change  of  supporting  leg.  In 
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the  absence  of  disturbances,  the  leg  angle  can  be  described  with  a  periodic  function,  6,  (?)  • 

The  existence  of  such  a  function  provides  a  "stable  gait".  However,  disturbances  do  occur 
in  locomotion  systems.  That  is  why  feedback  control  is  needed.  With  such  a  control 
mechanism,  the  leg  angle  differential  equation  becomes 


x     — 
s 


0    1 

/q 

xs~ 

us(k)S(t-kT) 


(eq.  2.60) 


where  5  denotes  the  unit  inpulse  function  and 

T 

xs  ~  (x3'*4) 


(eq.  2.61) 


It  is  shown  in  [GUBI74]  that  a  suitable  discrete  time  control  vector  for  this  system  is 


us(k)  =  h3[x3(kT)-Qs]  +  h4 


xAkT)—2 

r0 


+  e 


0 


(eq.  2.62) 


The  angle,  6   ,  is  the  desired  total  leg  angle  excursion  over  one  cycle,  while  Vq  is  the 

desired  forward  speed. 

Another  foot  placement  algorithm  for  controlling  forward  speed  is  investigated  by 
Raibert[RABI74].  In  this  work,  a  one  legged  hopping  machine  was  developed  to 
investigate  dynamically  balanced  running  robots.  The  machine  has  two  main  parts:  the 
body  and  the  leg  which  is  connected  to  the  body  with  a  hinge.  There  also  exists  a  hip 
actuator  to  be  able  to  apply  a  torque  from  the  body  to  the  leg. 
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Figure  6:  One  Legged  Hopping  Machine  with  Control  Variables  [RAIB86] 


Neutral  Point 


Figure  7:  Placing  The  Foot  at  The  Neutral  Point  [RABI74] 

For  each  forward  speed  there  is  a  foot  position  which  causes  zero  acceleration  in  the 
direction  of  motion.  This  point  is  called  the  "neutral  point".  Placing  the  foot  behind  the 
neutral  point  accelerates  the  machine,  placing  the  foot  in  front  of  the  neutral  point 
decelerates  it. 


20 


Acceleration 


Deceleration 


Neutral  Point 


Neutral  Point 


Figure  8:  Acceleration  and  Deceleration  of  the  one  Legged  Hopping  Machine  According 

to  Neutral  Point[RADB86] 

To  place  the  foot  at  the  neutral  point,  the  control  system  should  extend  the  leg  such 
that  [RAIB86] 

(eq.  2.63) 


X/o  "   ~2 


where  X  f   is  the  forward  displacement  of  the  foot  with  respect  to  the  center  mass,    x    is 
Jo 

the  forward  speed,  and  T     is  the  duration  of  the  stance  phase.  An  additional  displacement 


is  needed  to  correct  the  errors  in  the  forward  speed: 
X/a  =  kx^X~Xd^ 


(eq.  2.64) 


where  xf     is  the  displacement  of  the  foot  from  the  neutral  point,    x  >    is  the  desired 
/a  a 

forward  speed  and  k.  is  the  feedback  gain  constant. 

By  combining  the  equation  Eq.  2.64  and  Eq.  2.65,  the  total  foot  displacement  is 
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xTs 

Xf  =   —  +  ki(x-xd)  (eq.2.65) 

Then  the  required  hip  angle  for  the  desired  velocity  is 

fxT       kx(x-xd)\ 
yd  =  &-arcsin\—  + (eq.  2.66) 

where  y  is  the  angle  between  the  leg  and  the  body  [RAIB86]. 

Another  research  focused  on  biped  systems  was  conducted  by  Troy  [TROY96].  He 
investigated  locomotion  of  dynamically  balanced  biped  mechanisms.  Different  multilink 
planar  and  spatial  biped  models  were  developed  by  using  feedback  control  for  balancing 
and  walking.  In  general,  his  work  extends  the  results  described  above  while  confining 
locomotion  to  a  specified  sagittal  plane. 

G.      SUMMARY 

This  chapter  provides  a  survey  of  previous  work  relating  to  modeling  and  control 
of  posture  and  gait  for  a  walking  human.  It  starts  with  kinematic  modeling  and  continues 
with  single  and  articulated  rigid  body  dynamic  models.  One  section  discusses  simulation 
of  postural  control  for  erect  posture  of  the  body.  The  last  part  of  the  chapter  illustrates  two 
different  stepping  control  approaches  for  a  given  desired  forward  speed.  The  next  chapter 
of  this  thesis  derives  the  differential  equations  of  a  human  model  which  has  two  degrees  of 
freedom  by  using  the  Newton-Euler  method.  It  also  discusses  postural  control  for  the  same 
model. 
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III.  DETAILED  PROBLEM  STATEMENT  AND  MATHEMATICAL 

FORMULATION 


A.  INTRODUCTION 

The  model  in  [GUBI74],  Figure  4,  is  designed  to  simulate  postural  and  gait  control 
with  three  degrees  of  freedom.  The  body  attitude  and  the  ankle  angle  are  used  for  postural 
control  and  the  variable  leg  length  and  foot  placement  as  well  as  periodic  stepping  are  used 
for  gait  control.  The  model  has  one  input  control  torque  and  one  input  control  force. 
Another  similar  model,  dealing  only  with  postural  control,  will  be  introduced  in  this 
chapter.  This  model  has  two  degrees  of  freedom  with  the  same  variable  angles  of  the 
previous  model.  Instead  of  the  input  control  force,  this  model,  shown  in  Figure  9,  has  a 
second  input  control  torque  at  the  ankle. 

B.  DESCRIPTION  OF  THE  MODEL 


Figure  9:  Single  Rigid  Body  Model  with  A  Constant  Length  Massless  Supporting  Leg 
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This  model,  Figure  9,  has  a  single  rigid  body  with  one  supporting  massless  leg.  The 
system  has  two  degrees  of  freedom.  The  state  vector  for  the  system  is 

.       T 

x  =  (e1,e1,e2,e2)  (eq.3.u 

where  0,  is  the  leg  angle  and  62  is  the  body  attitude.  The  leg  length,  r  ,  is  constant. 

The  input  vector  for  the  state  equations  is 

T 
u  =  (Mj,  u2)  (eq.  3.2) 

where 

«j   =  Af  j  (eq.  3.3) 

and 

In  these  equations,  M,    is  the  control  torque  applied  at  the  ankle  and  Mr,   is  the  control 

torque  applied  at  the  hip. 

The  input  vector,  u ,  is  used  to  control  leg  angle  and  body  attitude.  According  to 
general  linear  feedback  control 

rM\\ 

U  =  [M    )   =  K(X~X0^  (eq.  3.5) 

where  Xq  is  the  desired  state  vector.  A  special  form  of  this  equation,  called  local  feedback, 

has  been  proposed  for  postural  control  [KOOZ83][CAMA77].  In  local  feedback,  each  joint 
torque  is  determined  by  angle  and  rotation  rate  of  that  joint  only.  This  decouples  control  of 
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joints  from  each  other.  Specifically,  in  this  form  of  control,  the  leg  angle  is  controlled  by 
U-,  as 

u\    =  kQ^Q\-a0  +  kQ^\  (eq.3.6) 

where     &fi     and     k.    are  gain  constants  and  a,    is  the  desired  leg  angle.  The  body 
attitude  is  controlled  by  u^  as 

u2  =  keS®2~a2^  +  ka  ®2  (eq.  3.7) 

where    kQ     and    k .     are  gain  constants  and    0Co  is  the  bias  term  to  permit  the  desired 
body  attitude.  Local  control  will  be  examined  further  in  this  thesis. 

C.      THE  LAGRANGIAN  VERSION  OF  THE  PROBLEM 

The  components  of  the  linear  velocity  vector  of  the  center  of  mass  of  the  single  rigid 
body  can  be  derived  from  angular  variables  as  follows: 

y  =  rGjCOsGj  + /92COS02  (eq.3.8) 

Z  =  -r6 1  sinGj-Z^sii^  (eq.  3.9) 

The  kinetic  energy  of  the  system  is  thus 

1  .  2      1-  2 

K  =  -mCrGjCOsGj  + /92cos02)    +  -mOrBisinOj-^sir^) 

+  ^/e2  (eq.3.10) 

The  potential  energy  of  the  system  can  be  expressed  as 

V  =  mgOcosGj  +  /cos02)  (eq.  3.11) 

Thus,  the  Lagrangian  function  becomes 
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1  •  2      1-  2 

L  =  -raOBjCosGj  +  /02cos99)    +  -m(-r8]  sii^-Zf^sinSj) 

1    -2 
+  -792  -  mgOcosGj  +  /cos62)  (eq.  3.12) 

The  differential  equation  for  the  first  generalized  coordinate,  9 ,  ,  is 

ddL       dL 


dtdQ    ae1 


=  Ml-M2  (eq.  3.13) 


dL 
The  term, ,  in  equation  Eq.  3.12  can  be  derived  as 

——  =  mr  SjCcosSj)2  + /mr82COs81cos82 

+  mr  SiCsinSj)2  + /mr62sin61sin82  (eq.  3.14) 

Then,  the  first  term  in  the  equation  Eq.  3.12  is 

ddL  2X   , o     ~       2A2 


dtdQ. 


=  mr  Qi(cosQ^)z -2mr  GjCOsO^inOj 


+  lmr§2  cos  9  j  cos  92-/mr929  1  sin  9  j  cos  99 

•  2  2-  2 

-  /rar92Cos9jSin99  +  mr  9i(sin9) 

2-2 
+  2mr  9]  sinGj  cos99  +  m/r92sinG]  sin99 

.2 
+  m/r929jCOs91sin92  +  ra/r92sin9j  cos99  (eq.  3.15) 


And  the  second  term  in  the  equation  Eq.  3. 12  is 
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rif  2-2 

=  -mr^Gj  sin 9 j  cos 6 j  -rar/9i92sin91cos99 


ae, 


2-2 
+  mr  9]COs91sin91  +  mr/0i92cos0]sin92 


+  ragrsin9j  (eq.  3.16) 

By  substituting  Eq.  3. 14  and  Eq.  3.15  intoEq.  3.12,  the  following  equation  can  be  obtained: 

2- 
mr  0]  +  /rar92(sin9jSin92  +  cos91cos92) 

.2 
+  /mr92(sin9j  cos99  -  cos9j  sin92) 

+  /rar929j(cos9jSin92  -  sin 9  j  cos92) 

+  Zmr9192(sin91  cos92  -  cos9j  sin92) 

-m^rsin91  =  M<[-M1  (eq.  3.17) 

After  applying  trigonometric  conversion  rules,  the  first  differential  equation  of  the  model 
can  be  derived  as  follows: 

2-  ••  .2 

mr  9j  +  /rar92COs(92  -9i)  +  /mr92sin(91  -  92) 

-mgr  sin  9  j   =  M]-M2  (eq.  3.18) 

The  differential  equation  for  the  second  generalized  coordinate,  99  ,  is 
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d  dL       dL 

-, —  —=?£-  -  Ml  (eq.  3.19) 


dL 

The  term, ,  equation  in  Eq.  3.19  can  be  derived  as 

ae2 


r)T  •  1  •  1 

—r-  =  ra/r91cos91cos92 +  ra/  92(cos92) 

2  ■  2 

+  m/rG1sineisin62 +  m/  92(sin92)    +^62  (eq.  3.20) 

The  time  derivative  of  Eq.  3.20  is 

ddL  ,  x  ~  „  ,  a2 


*ae- 


=  ra/r9  j  cos 9 ,  cos 92  -  mlrQ  j  sin 9 ,  cos 92 


■     •  2-  2 

-m/r9192COs91sin92  +  ml  92(cos92) 

2-2 
- 2ml  92COs92sin92  +  mlrQi sin9j sin92 

.2  .     . 

+  m/r9jCOs91  sin92  +  m/r0 1 02  sin  9 1  cos92 


+  ml  92(sin92)    +  2m/292sin92cos92  +  792  (eq. 3.21) 

Then  the  second  term  in  the  equation  Eq.  3.18  can  be  derived  as 

r)/ 

__  =  -m/r9192Cos91sin92  +  m/r9192sin91cos99 

+  rag/sin92  (eq.  3.22) 

By  substituting  Eq.  3.21  andEq.  3.22  into  Eq.  3.19,  the  following  equation  can  be  obtained: 
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.2 
m/r6]  cosGj  cos62  -  m/r9j  sin0j  cos02 


•     •  2-  2 

-  m/rO^cosGj  sin99  +  rar02(cos02) 

2-2 

-  2ml  02cos09sin0o  +  ra/r0j  sin0j  sin02 

.2 
+  ra/r0 1  cos  0 l  sin  02  +  ra/r0  j  02  sin  0  j  cos  09 


7  ■•  7  -2 

+  ml  02(sin02)    +  2m/202sin02cos02  +  702 


+  mlr  0 1 02  cos  0  j  sin  09  -m/r  0  j  02  sin  0  j  cos  09 


+  mg/sin09  =  M2  (eq.  3.23) 

Then,  the  second  differential  equation  of  the  model  can  thus  be  expressed  as  follows: 

.2 
m/r01cos(01  -  02)  +  m/r0]  sin (02-  0j) 

2  •• 
+  (I  +  ml  )02-mg/sin02  =  M2  (eq.  3.24) 

The  model  in  [GUBI74]  has  an  additional  degree  of  freedom  comparison  to  the 
model  presented  in  this  thesis  which  is  variable  leg  length  and  does  not  have  any  ankle 

control  torque.  If  the  time  derivative  values  of  the  leg  length  variable  r  are  taken  out,  and 
the  ankle  torque  M,  is  added  to  the  dynamic  equations  in  [GUBI74],  it  is  seen  that  these 
equations  are  identical  with  the  equations  Eq.  3.17  and  Eq.  3.23. 
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D.      NEWTON-EULER  FORMULATION 

As  discussed  in  the  previous  chapter,  if  the  Lagrangian  method  is  used,  an  increase 
in  the  number  of  degrees  of  freedom  complicates  the  calculations  exponentially  for  an 
articulated  mechanism.  Even  though  the  number  of  variables  are  not  many  in  this  case,  by 
considering  that  future  research  is  likely  to  investigate  more  complex  models,  the  Newton- 
Euler  method  looks  as  if  a  better  approach  for  this  kind  of  problems. 

The  model,  presented  in  this  chapter,  can  be  divided  into  two  parts  to  simplify  the 
problem.  These  parts  are  the  massless  leg  and  the  body  itself. 

1.    The  Newton-Euler  Equations  of  the  Massless  Leg 


Figure  10:  Free  Body  Diagram  for  The  Massless  Leg 
Because  the  leg  has  no  mass,  it  is  only  possible  to  talk  about  the  static  equilibrium 
of  this  body  part.  The  equilibrium  equations  can  be  expressed  as 
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I^  =  o 


i=  1 


and 


Y  M-F r = 0 


i=  1 
where  M-,  F  ■  and  r  are  defined  in  Figure  10. 

Equation  Eq.  3.25  can  also  be  written  as 


Ml-M2-rFr  =  0 


so  the  reaction  force,  F  ,  is  given  by 


(eq.3.25) 


(eq.  3.26) 


(eq.  3.27) 


F    = 

r 


Mx-M2 


(eq.  3.28) 


2.    The  Newton-Euler  Equations  of  the  Body 


Figure  1 1 :  Free  Body  Diagram  of  The  Body 
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The  dynamic  equations  of  the  free  body  in  the  z  and  y  directions  can  be  derived  as 
m'z  =  -mg  +  FcosGj  -Frsin61  (eq.  3.29) 

and 

my  =  FsinGj -Frcos01  (eq.  3.30) 

For  the  angular  motion  of  the  free  body,  the  dynamic  equation  of  the  body  can  be  expressed 


as 


/02  =  M2-/Frcos(e2-ei)  +  ZFsin(e2-G1)  (eq.3.3l) 

3.    Combining  Body  and  Leg  Equations 

As  shown  in  Figure  9,  the  geometric  constraint  equations  for  the  model  can  be 
derived  as 

3;  =  rsinGj  +  /sin92  (eq.  3.32) 

and 

Z  =  rcosGj  + /cos62.  (eq.  3.33) 

The  second  time  derivatives  of  Eq.  3.30  and  Eq.  3.31  are  as  follows: 

.2  ..  -2 

y  =  r 9 1  cos  6 ,  -  r0  j  sin 9 ,  +  /02  cos  62  -  /02  sin 92  (eq.  3.34) 

and 

.2  ..  .2 

z  =  -  rG^inGj  -  rGjCosOj  - /92sin90 -/92Cos6?.  (eq.  3.35) 

The  constraint  force  F  .  in  equation  Eq.  3.29  can  be  eliminated  by  using  equation  Eq.  3.26 
resulting  in  the  expression 
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l(M}  -M9)cos(69-61) 
/e2   =  M2 ■ — = -  +  ^8^(82-6!)  (eq.3.36) 

By  combining  Eq.  3.28  and  3.32,  the  following  equation  can  be  derived: 

.2 
m(rGiCOs9j  -  /-OjsinBj  +  /62Cos62  -/62sin99)  = 

=  FsinGj  +Frcos01  (eq.3.37) 

On  the  other  hand,  after  substituting  z  with  Eq.  3.33,  Eq.  3.27  can  be  written  as 

.2  ••  .2 

ra(-  rGj  sinGj  -  r0j  cosGj  -  /62sin62  -  /62Cos62)  = 

=  -rag  +  Fcos6j  -FrsinGj  (eq.  3.38) 

The  equation  Eq.  3.34  can  redefined  as 

MM  +  r)-lM, 

e2/-/r/sin(e2-e1)  =  — — ■ -cos(e2-9j)        (eq.3.39) 

After  reorganizing,  Eq.  3.35  and  Eq.  3.36  become 

GjrarcosGj  +  62m/cos62- FsinGj  = 

.2                        .2                  M\~M2 
=  rarGjsinGj  +  m/G2sinG9  + cosGj  (eq.  3.40) 

and 

GimrsinGj  +  G2w/sin62  +  FcosG!  = 

.2                      -2                             M]~M2 
=  -mrG1cosG1-m/G2COsG2  +  mg  + sinGj         (eq.  3.41) 

Equations  Eq.  3.37,  Eq.  3.38,  and  Eq.  3.39  can  be  expressed  in  matrix  form  as: 
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0 


mrcosGj  ra/cosG2 
rarsinGj  ra/sinG2 


/        -/sin(92-ei) 
-sin  6 


cos  6 


l 


e2 

F 


M2{l  +  r)-lMx 


cos(92-  Bj) 


M,  -M, 


.2                      .2                ir±\~1Y±2 
rarGjsinGj  +  m/02sin62  + cosGj 


Mx-M2 


(eq.  3.42) 


.2                       .2 
rarOjcosGj  -m/62cos02  +  mg  h sinGj 


An  alternative  to  this  analytic  formulation  of  the  Newton-Euler  formulation  is  to 
use  the  recursive  approach  explained  in  Chapter  II  of  this  thesis.  Thus  results  in  more 
complex  logic,  but  in  less  computation,  especially  as  the  number  of  links  in  the  dynamic 
model  increases. 

E.      LINEARIZED  ANALYSIS  FOR  CHOOSING  GAINS 

As  mentioned  in  the  previous  sections,  this  system  has  two  inputs,  hip  and  ankle 
torques.  The  main  purpose  of  postural  control  is  to  determine  these  input  torques  in  order 
to  maintain  the  upright  orientation  of  the  body.  The  approach  taken  in  this  thesis  is  linear 
state  feedback  control.  The  control  equations,  Eq.  3.5  and  Eq.  3.6,  are  presented  in  the 
second  section  of  this  chapter.  For  the  local  control  model,  the  problem  is  to  compute  the 

required  gain  values:  ka  ,  k-    ,  ka  ,  k,   . 

yi      6i      y2     e2 

It  is  reasonable  to  drop  quadratic  velocity  components  for  small  motion  linearized 

.2  -2 

analysis  [MCGH86].  Under  this  assumption,  6|  and  62  -  components  are  removed  from 
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all  equations.  The  same  assumption  means  that  61   and  G9  can  have  only  small  values 
which  allows  the  substitutions 

sinBj   =  Gj,  (eq.3.43) 

sin62  =  62,  (eq.  3.44) 

COS01    =    1,  (eq.  3.45) 

and 

COS02  =    1  •  (eq.  3.46) 

With  these  considerations,  equation  Eq.  3.18  can  be  rewritten  as 

2- 
mr  Q\+lmrQ2  =  mgrQ^+  M^-M2.  (eq.  3.47) 

The  linearized  version  of  equation  Eq.  3.24  is 

2   ■• 
mlrQi+(I+ml  )82  =  mg/G9  +  M2  (eq.  3.48) 

02  can  be  evaluated  from  equation  Eq.  3.47  as 

2- 
mgrQ,  +  M}  -  M7  -  mr  Oj 

62  = (eq.  3.49) 

Imr 

If  equation  Eq.  3.49  is  substituted  in  equation  Eq.  3.48,  the  following  equation  is  obtained 

(mlr)2Ql  +  (I+ml2)(mgrQl+Ml-M2-mr2Ql) 

2     2 
=  rm  gl  62  +  m/rM2  (eq.  3.50) 

After  reorganizing,  equation  Eq.  3.50,  Gj  can  be  defined  as 
6i  = 
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2 
l2m2rgQ7  -  mgr(I  +  ra/2)9 j  -(I+ml2)Ml  +(lmr  +  I+ml  )M2 

—Imr 

(eq.3.51) 

The  angular  acceleration,  6  \  ,  can  be  defined  by  using  equation  Eq.  3.47  as 

mgrQ,  +  M,  -  M1  -  ImrQj 

91  =  i L-^ (eq.3.52) 

mr 

After  substitution  of  9  \ ,  equation  Eq.  3.48  can  be  rewritten  as 

/  2   •• 

-(mgrQl  +M1  -M2  - /mr62)  +  (/+ w/  )92  =mgIQ2  +  M2  (eq.   3.53) 

After  reorganizing  equation  Eq.  3.53,  9 2  can  be  define  as 

92  = 

mglrQ7  -  ImgrQ ,  -  /M,  +  (r  +  /)M0 

— (eq.3.54) 

The  general  linear  state  feedback  equation  is 

x  =  Ax  +  Bu  (eq.  3.55) 

The  state  vector,  x,  is  defined  by  equation  Eq.  3.1.  If  the  desired  values  for  the  angles  are 
chosen  as  zero,  the  input  vector,  in  equation  Eq.  3.5,  can  be  rewritten  as 

u  =  Kx  (eq.  3.56) 

After  substituting  u  according  to  the  equation  Eq.  3.56,  Eq  3.53  can  be  written  as 
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x  =  (A  +  BK)x 


(eq.  3.57) 


where 


A  = 


0 


1        0       0 


2         2    2 
mgr(I  +  ml  )      /  m  rg 


Imr 

0 

-Imgr 

Ir 


Imr 
0        0 


1 


o  ^-r  0 

Ir 


and 


5  = 


0 

0 

I  +  ml 

2 
-(Imr  +  I  +  ml  ) 

Imr 

Imr 

0 

0 

/ 
Ir 

r  +  / 

(eq.  3.58) 


(eq.  3.59) 


For  local  feedback,  7£  can  be  defined  as  follows,  according  to  the  equations  Eq.  3.2,  Eq. 
3.6  and  Eq  3.7 


K  = 


-K*  -k.      0       0 
yi        8i 

0         0     -K*   -K, 


(eq.  3.60) 


where  desired  angle  values  Ot,   and  OU  are  taken  as  zero.  Then,  by  using  equation  Eq. 
3.58,  Eq.  3.59  and  Eq.  3.60,  A  +  BK  can  be  defined  as 
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A  +  BK  = 


0 

l 

0  0 

M  N  0  P 

0 

0 

0   1 

Q 

V 

Y  Z 

(eq.  3.61) 


where 


M  = 


2  2 

mgr(I  +  ml  )  -  Kq  (I  +  ml  ) 

Imr 


(eq.  3.62) 


N  = 


-K    (I  +  ml  ) 
t>i 

Imr 


(eq.  3.63) 


2     2  2 

-/  m  rg  +  Ka  (lmr  +  I  +  ml  ) 

o  = -±j 

Imr 


(eq.  3.64) 


P  = 


K.  (lmr  +  I  +  ml  ) 
62 


Im/ 


(eq.  3.65) 


Q  = 


-  Imgr  +  IKq 


rl 


(eq.  3.66) 


V  = 


V 

r/ 


(eq.3.67) 


Y  = 


mglr  -  Kq  (r  +  I) 


rl 


(eq.  3.68) 


Z  = 


-K.{r  +  l) 

"2 

^7 


(eq.  3.69) 
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For  a  stable  upright  posture,  all  roots  of  the  characteristic  equation  (eigenvalues) 
should  have  negative  real  parts  [KU095].  The  characteristic  equation  of  the  closed  loop 
system  is  [KU0951 

\sI-(A  +  BK)\  =  0  (eq.3.70) 

After  substituting  the  equation  Eq.  3.60  the  equation,  Eq.  3.69  can  be  rewritten  as 


s  -1 

0 

0 

C  D 

E 

F 

0    0 

s 

-1 

G  H 

W 

Y 

=  0 


(eq.3.71) 


where 


c  = 


2  2 

-mgr(I  +  ml  )  +  Kq  (I  +  ml  ) 

Imr 


(eq.  3.72) 


D  =  s  + 


K    (I  +  mf) 
Imr 


(eq.  3.73) 


E  = 


2    2  2 

/  m  rg-K^  (Imr  +  I  +  ml  ) 

Imr 


(eq.  3.74) 


F  = 


-K.  (Imr  + 1  +  ml  ) 

02 


Imr 


(eq.3.75) 


Imgr  -  Kq  I 

G  = 7T^ 


(eq.  3.76) 
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-v 

rl    ' 


H  =  — ,  (eq.  3.77) 


-Imgr  +  Kn  (r  +  /) 

W  =  — ,  (eq.3.78) 

rl 


K,(r  +  l) 

Y  =  s  + 2—T (eq.  3.79) 

rl 

If  the  determinant  of  in  the  equation  Eq.  3.69  is  evaluated,  the  following  polynomial  is 
obtained: 

4     2 
s  (r  Im) 

2>f  2        2  ^ 

+  s     IrmK.    +  IK.    +  K.ml   +r  mK. 

V  ©2  ©I  01  02/ 

2  2     2  2  2     2 

+  s  (IKq  -I  m  rg  +  ml  Kq  -r  m  Ig 

2 
+  r  mKa   +rmlKQ  +K-K-    -  Imgr) 
e2  02  01     02 

+  s(K6K.-lmgK^  +  K.Ke2-mgrK.) 

+  (m  g  rl-  mgrKQ^  -  KQ  Img  +  KQ  KQ^  J  =  0  (eq.  3.80) 

It  is  clearly  a  very  hard  problem  to  determine  the  required  gain  values  in  equation  Eq.  3.80, 
which  result  in  all  roots  having  negative  real  parts.  Instead,  a  experimental  solution  was 
found  by  trying  some  gain  values  on  the  computer  model.  It  was  found  that  the  following 
values  produce  an  upright  stable  body  posture: 
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KQi  =  25000,  (eq.3.81) 

K.  =  2500,  (eq.3.82) 

K^  =  4000,  (eq.3.83) 

K.  =  400  (eq.3.84) 

F.      SUMMARY 

In  this  chapter,  a  new  human  dynamic  model,  which  is  made  of  a  body  and  a 
massless  leg,  is  introduced.  The  dynamic  differential  equations  of  the  model  are  derived  by 
using  two  different  methods:  the  Lagrangian  and  the  Newton-Euler  methods.  The  last 
section  of  the  chapter  discusses  the  small  motion  linearized  analysis  of  the  system.  It  is 
explained  how  the  gain  constants  for  the  input  torques  should  be  chosen. 

The  next  chapter  will  explain  the  implementation  of  computer  models  based  on  the 
knowledge  presented  in  the  second  and  third  chapters  of  this  thesis. 
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IV.  COMPUTER  MODELS 

A.  INTRODUCTION 

In  the  second  chapter  of  this  thesis,  it  is  pointed  out  that  there  exist  two  major 
methods  to  simulate  human  motion  simulation:  kinematic  and  dynamic  models.  The 
present  chapter  first  presents  stepping  algorithms  for  a  human  figure  implementation  which 
is  developed  by  using  a  kinematic  model.  The  second  chapter  also  discusses  the  general 
approaches  for  dynamic  simulation  of  human  motion.  Based  on  this  knowledge,  the  third 
chapter  investigates  the  mathematical  representation  of  a  dynamic  human  model.  The 
present  chapter  also  introduces  the  computer  implementations  of  these  models. 

B.  KINEMATIC  COMPUTER  MODEL  (DYNAMAN) 


Figure  12:  Kinematic  Computer  Model:  Dynaman 
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The  kinematic  model  presented  in  this  section  is  made  of  fifteen  separate  body  parts 
as  shown  in  Figure  13. 

DCS1 


DCS  11 


DCS  14 


DCS  5 

DCS  A 
DCS  9 


DCS  2 

DCS  1 
DCS  8 


Figure  13:  The  Body  Parts  and  Corresponding  Dynamic  Coordinate  Systems  (DCS)  of 

Dynaman 

The  purpose  of  the  simulation  is  to  create  a  computer  representation  of  a  stepping 
human.  Under  this  requirement,  it  is  clear  that  the  main  concern  is  leg  and  foot  motion.  The 
other  body  parts  are  synchronized  according  to  the  legs. 

Inverse  kinematics  is  chosen  for  computing  leg  motion.  Inverse  kinematic 
equations  take  the  position  of  the  end  effector  (foot)  as  the  input  and  computes  each  joint 
angle  of  the  leg.  The  points  which  describe  the  position  of  the  foot  in  space  for  a  full  gait 
period  produce  a  path.  The  question  is  to  define  this  path  as  a  mathematical  function  of 
time.  This  function,  of  course,  should  be  developed  with  the  knowledge  of  the  geometry  of 
the  environment,  such  as  height  of  each  stair. 

Forward  kinematics  could  have  been  chosen  for  the  same  problem.  In  that  case, 
joint  angles  of  the  leg  would  be  the  input  to  the  forward  kinematic  equations,  and  the 
position  of  the  end  effector  would  be  calculated.  For  this  second  case,  there  should  exist  a 
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feedback  system  in  order  to  input  the  constraints  of  the  environment  to  the  model.  A 
collision  detection  mechanism  between  the  foot  and  the  floor  seems  to  be  appropriate  to  this 
approach  [GOET94]. 

1.    Inverse  Kinematic  Equations  for  Three  Link  Planar  Manipulator 


Figure  14:  Three  Link  Planar  Manipulator 

The  leg  of  Dynaman  can  be  thought  as  a  three  link  planar  manipulator  which  is 
made  of  the  upper  leg,  the  lower  leg,  and  the  foot.  The  frames  which  are  attached  to  the 
links  are  shown  in  Figure  14.  The  general  form  of  the  transformation  matrix  to  represent  a 

point  in  the  frame  i  —  1  which  is  defined  in  frame  i  is  [CRAI89] 

1-\t  = 
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(eq.4.1) 


where  a ,  OC ,  G ,  and  J  are  the  link  parameters  as  defined  in  the  second  chapter  of  this 
thesis.  The  link  parameters  for  the  presented  model  are  defined  in  Table  1 . 
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Table  1.  Link  Parameters  of  the  Leg  [CRAI89] 


In  this  model,  all  z  ■  axes  are  parallel,  and  all  X-  axes  are  in  the  same  plane.  That  is 
why  all  the  OC  •  _  i  and  d  ■  values  are  zero.  The  parameters  L  and  U  are  the  link  lengths 

of  the  first  and  second  links.  The  parameter  L  defines  the  position  of  the  toe  point.  Since 

it  is  in  the  end  effector  frame,  it  is  not  included  in  the  link  parameters. 

According  to  equation  Eq.  4.1  and  Table  1,  the  transformation  matrices  for  the 
neighbor  links  are 


0 


T  = 


cos  6  j 

-sinGj  0  0 

sinGj 

cosGj   0  0 

0 

0       1  0 

0 

0       0  1 

(eq.4.2) 


46 


J  = 


cos02 


T  = 


-sin62  0  /j 

sin69    cos92  0  0 

0  0       10 

0  0       0  1_ 

cos03  -sinG3  0  L 

sin  63    cos  63   0  0 

0  0       10 

0  0       0  1 


(eq.4.3) 


(eq.  4.4) 


The  transformation  matrix  between  the  first  and  the  last  links  can  be  derived  by  using  the 
product 

0  0       1      2 

3T  =    {T    2T  3T  (eq.4.5) 

If  the  matrix  multiplications  are  executed  after  substituting  equations  Eq.  4.2,  Eq.  4.3,  and 
Eq.  4.4  into  the  equation  Eq.  4.5,  the  following  transformation  matrix  will  be  obtained: 

008(8^62  +  63)  -sin(6j  +  62  +  63)  0    /1cos61+/2cos(61  +  62) 

8111(6^62  +  63)    008(6!  +  62  +  63)    0    /1sin61  +  /2sin(61  +  62) 

0  0  10 

0  0  0  1 

(eq.4.6) 
If  the  method  in  [CRAI89]  is  used,  the  transformation  matrix  of  the  end  effector 
according  to  the  base  link  can  be  defined  as 
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0 


T  = 


cos0  -sin 6  0  x 

sinO    cosG  0  y 

0         0      10 

0         0      0  1 


(eq-  4.7) 


where  the  position  components,  x  and  y ,  and  the  orientation,  6 ,  of  the  end  effector 
according  to  the  base  link  are  known.  The  following  equations  can  be  derived  from 
equations  Eq.  4.6  and  Eq.  4.7: 

cosO  =  cos(0j  +  62  +  03)  (eq.  4.8) 


sinG  =  sinCOj  +02  +  63) 
x  =  /jCosOj  +  /2cos(0f  +  02) 
y  =  ZjSinOj  +  l2sin(Q^  +  02) 


(eq.  4.9) 
(eq.4.10) 
(eq.4.11) 


If  the  squares  of  equations  Eq.  4.10  and  Eq  4.11  are  added,  the  following  equation  is 
obtained: 


2        2         2       2 
x    +y    =  /j  +  /2  +  2/j/2cos02 

In  equation  Eq.  4.12,  the  only  unknown  is  02  and  it  can  be  defined  as 


(eq.4.12) 


02  =  acos 


-  2        2      .  2      .  2N 
<x    +y    -/j    -l2    ) 


(eq.  4.13) 


After  having  found  09  ,  equations  Eq.  4.10  and  4.1 1  can  be  can  be  written  as 


jc  =  &1cos01  -  /^sinOj 


(eq.  4.14) 


and 
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y  =  ^jSinGj  +  k2cosQ]  (eq.  4.15) 

where 

kx   =  /j  +/2COS09  (eq.  4.16) 


and 


k2  =  /2sin92  (eq.  4.17) 

After  following  the  steps  in  [CRAI89],  9 ,  is  defined  by  the  equation 


9,   =  atan  -  hatan 


e> 


(eq.4.18) 


Then  foot  angle  can  then  be  calculated  as 

93   =   9-9^92  (eq.4.19) 

2.    Link  Descriptions  in  the  Computer  Model 

The  computer  model  has  been  developed  in  the  IRIS  Performer™  application 
environment.  This  environment  allows  the  programer  to  create  a  Dynamic  Coordinate 
Systems  (DCS).  DCS  lets  the  programer  to  change  the  transformation  of  the  objects  which 
are  attached  to  that  DCS  node.  The  method  followed  in  this  application  is  to  attach  each 
body  part  to  a  DCS  node  and  connect  all  the  DCS  nodes  in  a  tree  structure  which  creates  a 
hierarchical  structure.  For  example,  if  the  upper  leg  is  rotated  for  some  degrees,  all  the  body 
parts  under  that  DCS  node,  lower  leg  and  foot,  follow  this  rotation.  DCS  tree  structure  of 
the  whole  body  is  shown  in  Figure  13  and  Figure  15. 
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DCS  15      DCS  6       DCS  7       DCS  1 


DCS  4 
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T  T  T 

DCS  13  DCS  16  DCS  8  DCS  9 

Figure  15:  Dynamic  Coordinate  System  (DCS)  Hierarchy  Tree  of  Dynaman 

The  body  parts  of  the  Dynaman  are  taken  from  an  other  model  developed  in 

OpenGLR  by  Will  Frey  at  the  Naval  Postgraduate  School  [FREY96].  However,  while  Frey 
used  body  segment  Euler  angles  relative  to  an  Earth  fixed  reference  system,  as  described 
above,  the  work  of  this  thesis  is  based  on  joint  angles.  The  coordinates  of  the  polygons, 
taken  from  the  other  model,  which  produces  the  body  parts,  are  loaded  to  the  application 
by  using  poly  format  files. 


3.    Stepping  Algorithms 

The  inverse  kinematics  model  takes  position  and  orientation  of  the  end  effector  and 
computes  each  joint  angle.  Then,  the  question  is  to  define  the  path  of  the  end  effector  (foot) 
which  is  the  input  to  the  inverse  kinematics  model.  Two  separate  algorithms  are  developed 
to  define  the  path:  stepping  forward  and  stepping  upward  algorithms. 

a.    Stepping  Forward  Algorithm 

The  leg,  without  bending  at  the  knee,  can  be  assumed  as  a  simple  swinging 

bar  on  a  circular  path  which  is  shown  as  P,   in  Figure  16.  The  angle  between  the  vertical 
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P2  =  p  j  cos  a 


and  the  swinging  bar  is  defined  as  OC ,  which  takes  values  between  -20  and  +20  degrees 
during  the  gait  cycle.  In  reality,  the  knees  are  bent  during  stepping.  Larger  values  of  a 

introduce  larger  bending  angles  at  the  knee.  When  OC  is  equal  to  zero,  knee  bending  angle 
should  also  be  zero.  This  requirement  is  introduced  by 

(eq.  4.20) 

There  is  a  difference  between  the  supporting  and  the  recovery  leg  motions.  While 
the  supporting  leg  always  touches  the  ground,  the  recovery  leg  should  be  moved  without 
touching  the  ground.  This  requirement  is  added  to  the  model  by  using  the  following 
equation  for  the  recovery  leg 

P3   =  0.95  P2  (eq.4.21) 


Figure  16:  Forward  Stepping  Algorithm 
No  kinematic  computation  is  needed  for  the  arms.  They  are  synchronized 
with  the  legs.  The  arm  rotation  is  not  exactly  the  same  as  leg  rotation;  a  scale  factor  is 
applied  for  a  more  realistic  looking  arm  motion.  Another  property  of  the  model  is  the 
rotation  of  the  upper  body  around  the  vector  described  by  the  general  direction  of  motion. 
This  motion  is  caused  by  the  roll  moments  which  are  generated  by  the  nature  of  biped 
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locomotion  since  the  supporting  legs  have  offset  from  the  center  of  mass  on  opposite  sides 
and  are  alternating  during  each  step  cycle. 


Figure  17:  Upper  Body  Rotation 
The  distance  from  the  upper  body  to  the  ground  is  not  constant.  When  the 
OC  parameter  has  larger  values,  the  whole  body  gets  closer  to  the  ground.  This  property  is 
implemented  by  moving  the  whole  body  vertically  with  a  cosine  function  of  time. 


Figure  18:  Change  in  the  Height  of  the  Body  During  One  Gait  Cycle 
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The  height  of  the  upper  body  is  calculated  by 
Height  =  0.08cos(27i/f) 


(eq.  4.22) 


b.    Stepping  Upward  Algorithm 

The  stepping  up  algorithm  has  a  major  difference  from  the  forward  stepping 
which  is  caused  by  the  need  for  increase  in  body  elevation 


Figure  19:  The  Amount  of  Elevation  For  Stepping  Up 

The  needed  elevation  for  stepping  up  is 

h  =   (/j  +/0)-(/1  +/2)cosp.  (eq.  4.23) 

This  elevation  can  only  be  handled  by  raising  the  foot  to  a  higher  position  in  the  front.  This 
causes  the  change  which  is  shown  in  Figure  20.  The  values  of  0C  change  from  -10  to  +30 
degrees  for  the  gait  cycle. 
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Figure  20:  Stepping  Up  Algorithm 
C.      DYNAMIC  COMPUTER  MODELS 

1.    Newton-Euler  Rigid  Body  Class 

The  Dynamic  models  in  this  thesis  are  developed  in  ANSI  Common  Lisp.  The  main 
class  for  the  dynamic  simulation  is  rigid-body  class  which  was  written  by  Professor 
McGhee.  This  Lisp  code  for  this  class  is  included  as  Appendix  A  of  this  thesis.  This  class 
defines  a  free  rigid  body  in  space.  The  major  method  of  the  rigid-body  class  is  update-rigid- 
body  which  updates  the  posture  vector  which  includes  the  position  and  the  orientation  of 
the  rigid  body  in  an  earth  coordinate  system.  This  method  converts  body  velocity  rates  to 
earth  velocity  rates  to  update  the  six  element  posture  vector.  Euler  integration  is  used  to 
update  body  velocities  by  using  body  velocity  growth  rates.  The  body  velocity  growth  rates 
result  from  applied  forces  and  torques  on  the  body.  The  linear  velocity  growth  rate  is 
computed  by  [FRAN69] 
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(eq.  4.24) 
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where  /  ,  /  ,  /    is  the  force  vector  applied  to  the  body,  ( u,  v,  w)  is  the  linear  velocity 

jl         y        4, 

vector,  (p,  q,  r)  is  the  rotational  velocity  vector,  (((),  0,  V|/)   is  the  body  orientation 

defined  in  Euler  angles,  m  is  the  mass,  and  g  is  the  gravitational  acceleration.  All  these 
vectors  are  defined  in  a  body  principal  axis  coordinate  system  [FRAN69].  The  angular 
velocity  growth  rate  is  computed  by 
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(eq.  4.25) 


where  (L,  M,  N)  is  the  torque  vector  applied  to  the  body,  /     ,  /     ,  and  /      are  mass 

moments  of  inertia,  (p,  q,  r)  is  the  rotational  velocity  vector  [FRAN69].  In  summary,  the 
update-rigid-body  method  calculates  the  new  position  and  orientation  of  the  free  rigid  body 
in  an  earth  coordinate  system  according  to  the  applied  forces  and  torques  to  the  body.  An 
other  important  method  is  move  which  takes  the  displacement  components  and  orientation 
Euler  angles  as  input  and  moves  the  body  according  to  the  inputs. 
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2.  Numerical  Integration  Methods 

Two  different  integration  methods  are  used  in  the  dynamic  modeling 
implementations:  Euler  and  Huen  integration.  The  Euler  integration  approach  can  be 
formalized  as  [KREY88] 

Si+1   =  *n +  /<**' *n>Al  (eq.4.26) 

where 

y  =  f(xn,  t)  (eq.  4.27) 

and  At  is  a  constant  increment  in  the  independent  variable,  t. 

The  Heun  integration  formula  can  be  defined  with  the  equation  [KREY88] 

'    _l  1    =   X    +r[f(x  ,t)  +  fix    ^  ,*    t        t)]A  (eq.  4.28) 

n+1  n      2  n     n  n+1        «+l/J  M  y 

where 

The  symbols  At  and  f(x,  t  )  have  the  same  definitions  for  Heun  integration  as  in  Euler 
integration. 

3.  Dynamic  Inverted  Pendulum  Simulations 

Three  different  dynamic  inverted  pendulum  models  are  implemented  in  this  thesis: 

a.    A  Single  Link  Single  Rigid  Body  with  Newton-Euler 

The  second  implementation  simulates  an  inverted  pendulum  by  using  Newton- 
Euler  formulation  of  the  dynamic  equations  of  the  system.  The  constraint  forces  from  the 
dynamic  equations  and  the  control  torque  are  the  inputs  of  the  rigid-body  class.  The  3x3 
matrix  multiplication  form  of  the  system  equations  is  as  follows: 
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(eq.  4.30) 


where  all  the  variables  are  defined  as  in  Figure  21. 


Figure  21:  Inverted  Pendulum  with  Constraint  Forces 
The  system  has  only  one  degree  of  freedom.  A  suitable  state  vector  is 

x  =   [0,0]  (eq.4.31) 

To  assume  M  =  0 ,  makes  the  system  behave  like  a  natural  inverted  pendulum 
without  control.  By  using  linear  state  feedback,  the  control  moment  can  be  determined  as 


M  =  ~K^-K.$ 


(eq.  4.32) 


where  KA  and  K .    are  control  gain  variables.  Suitable  values  for  K A  and  K .   can  be 
9  (p  y  (t> 

found  analytically  for  small  motion  linearization  of  this  system  because  its  characteristic 

equation  is  quadratic.  The  Lagrangian  formulation,  Eq  2.  27, is  most  useful  for  this  purpose. 
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b.  Massless  Leg  and  a  Single  Rigid  Body  with  Lagrangian 

The  second  implementation  simulates  a  two  link  structure  which  is 
presented  in  the  third  chapter  of  this  thesis.  The  dynamic  equations  of  the  system  are 
defined  with  Eq.  3.18  and  Eq.  3.24  which  are  derived  by  using  Lagrangian  method.  The 
linear  state  feedback  control  equations  are  given  by  the  equations  Eq.  3.6  and  Eq.  3.7.  The 
state  vector  is  defined  as  Eq.  3.1. 

c.  Massless  Leg  and  a  Single  Rigid  Body  with  Newton-Euler 

The  third  implementation  simulates  the  same  two  link  structure  which  is 
simulated  in  the  second  simulation.  However  the  Newton-Euler  method  is  used  to  derive 
the  dynamic  equation  instead  of  the  Lagrangian.  The  3x3  matrix  form  of  the  dynamic 
equations  are  given  by  Eq.  3.42.  The  linear  state  feedback  control  equations  and  the  state 
vector  are  defined  as  the  same  as  in  the  Lagrangian  version  of  the  simulation. 

D.      SUMMARY 

This  chapter  presents  forward  and  upward  kinematic  stepping  algorithms,  of  an 

human  model  which  is  developed  at  the  IRIS  Performer  application  environment  by 
using  C++.  The  second  part  of  the  chapter  explains  three  different  implementations  to 
simulate  various  inverted  pendulum  models  developed  in  Lisp.  The  next  chapter  discuses 
the  results  of  these  simulations. 
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V.  RESULTS  OF  COMPUTER  SIMULATIONS 

A.  INTRODUCTION 

In  this  chapter,  the  results  of  the  dynamic  simulations  are  presented.  The  second 
part  of  the  chapter  contains  frames  from  the  kinematic  simulation  of  Dynaman  which  show 
the  model  stepping  forward  and  upward. 

B.  DYNAMIC  SIMULATIONS 

1.    A  Single  Link  Single  Rigid  Body  with  Newton-Euler  Method 

Figure  22,  Figure  23,  and  Figure  24  show  the  behavior  of  the  inverted  pendulum 
with  a  control  torque  at  the  pivot  point.  The  dynamic  equations  of  the  system  are  derived 

by  using  the  Newton-Euler  Method.  The  mass  of  the  rigid  body,m ,  is  100  lb.  The  body 
rotary  inertia,  /,  is  900  lb.  ft.  .  The  length  of  the  inverted  pendulum,  /,  is  3  ft.  The 

gravitational  acceleration,  g ,  is  32.2185  ft.  /  sec.  .  The  gain  values  for  the  control  torque 
are 

Ke  =    10000  (eq.5.1) 

K.   =  2000  (eq.5.2) 

0 

The  initial  state  vector  is 

x  =   (1,0)  (eq.5.3) 

and  the  state  vector  in  steady  state  is 

X  =   (0,0)  (eq.5.4) 

Figure  25  shows  inverted  pendulum  orientation  change  in  time.  Euler  integration 
with  a  time  step  of  0.02  seconds  was  used  for  the  results  shown  in  Figure  22-25. 
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strobe-cam  era-image 


Figure  22:  Initial  Position  and  Orientation  of  the  Inverted  Pendulum 


strobe-cain  era-image 


Figure  23:  Inverted  Pendulum  Moving  to  the  Upright  Orientation 
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strobe-cam  era-image 


Figure  24:  Inverted  Pendulum  After  Steady  State 


Body  Attitude  (theta)  NEWTON-EULER 


Figure  25:  Body  Attitude  Response  of  the  Inverted  Pendulum 
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2.    Massless  Leg  and  a  Single  Rigid  Body  with  Lagrangian  Method 

Figure  26,  Figure  27,  Figure  28,  and  Figure  29  show  the  behavior  of  the  two  link 
inverted  pendulum  with  control  torques  at  the  hip  and  at  the  ankle.  The  z  axis  of  the  body 
is  drawn  to  be  able  to  observe  the  body  attitude  and  the  leg  angle  separately.  The  dynamic 
equations  of  the  system  is  derived  by  using  the  Lagrangian  Method.  The  mass  of  the  rigid 

body,m ,  is  100  lb.  The  body  rotary  inertia,  /,  is  100  lb.  ft.2.  The  length  of  the  leg,  r ,  is  3 

ft.  The  length  of  the  rigid  body,  / ,  is  0.5  ft.  The  gravitational  acceleration,  g ,  is  32.2  ft.  / 

sec.  .  The  gain  values  for  the  control  torques  are 

(eq.5.5) 

(eq.5.6) 

(eq-  5.7) 
(eq.5.8) 

The  initial  state  vector  is 

x  =  (0.5,0,  1,0)  (eq.5.9) 

and  the  state  vector  in  steady  state  is 

X  =   (0,0,0,0)  (eq.5.10) 

Figure  30  and  Figure  3 1  show  body  attitude  and  leg  angle  changes  in  time.  Heun 
integration  with  a  time  step  of  0.0 1  seconds  was  used  for  the  results  shown  in  Figure  26-3 1 . 
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Two  Link  Inverted  Pendulum  (Constant  Length  Massless  Leg)  LAGRANGIAN 


Figure  26:  Initial  Orientation  of  the  Two  Link  Inverted  Pendulum  (Lagrangian) 


Figure  27:  Two  Link  Inverted  Pendulum  Moving  to  the  Upright  Orientation  by  Control 

Torques(Lagrangian) 
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Figure  28:  Two  Link  Pendulum  Recovering  the  Negative  Orientations  (Lagrangian) 


Two  Link  Inverted  Pendulum  (Constant  Length  Massless  Leg)  LAGRANGIAN 


Figure  29:  Two  Link  Inverted  Pendulum  After  Steady  State  (Lagrangian) 


64 


LI,/:;:    ,:;:::      ;y: ^U&                 U    1 

Body  Attitude  (theta-2)  LAG  RANG  IAN 
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Figure  30:  Body  Attitude  Response  of  the  Two  Link  Inverted  Pendulum  (Lagrangian) 


Leg  Angle  (theta-1)  LAGRANGIAN 


Figure  31 :  Leg  Angle  Response  of  the  Two  Link  Inverted  Pendulum  (Lagrangian) 
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3.    Massless  Leg  and  a  Single  Rigid  Body  with  Newton-Euler  Method 

Figure  32,  Figure  33,  Figure  34,  and  Figure  35  show  the  behavior  of  the  two  link 
inverted  pendulum  with  control  torques  at  the  hip  and  at  the  ankle.  Again,  the  z  axis  of  the 
body  is  drawn  to  be  able  to  observe  the  body  attitude  and  the  leg  angle  separately.  The 
dynamic  equations  of  the  system  are  derived  using  the  Newton-Euler  Method.  The  mass  of 

the  rigid  body,m ,  is  100  lb.  The  body  rotary  inertia,  /,  is  100  lb.  ft.2.  The  length  of  the  leg, 

r ,  is  3  ft.  The  length  of  rigid  body,  /,  is  0.5  ft.  The  gravitational  acceleration,  g ,  is  32.2  ft. 

/  sec.  .  The  gain  values  for  the  control  torques  are 

(eq.5.11) 

(eq.5.12) 

(eq.5.13) 

(eq.5.14) 

The  initial  state  vector  is 

x  =  (0.5,0,  1,0)  (eq.5.15) 

and  the  state  vector  in  steady  state  is 

X  =   (0,0,0,0)  (eq.5.16) 

Figures  32  through  37  show  body  attitude  and  leg  angle  changes  in  time.  Heun 
integration  with  a  time  step  of  0.01  seconds  was  used  for  these  results. 
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lVo  Link  Inverted  Pendulum  (Constant  Length  Massless  Leg)  NEWTON-EULEF 


Figure  32:  Initial  Orientation  of  the  Two  Link  Inverted  Pendulum  (Newton-Euler) 
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Two  Unk  Inverted  Pendulum  (Constant  Length  Massless  Leg)  NEWTON-EULE 


Figure  33:  Two  Link  Pendulum  Moving  to  the  Upright  Orientation  (Newton-Euler) 
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rwo  Link  Inverted  Pendulum  (Constant  Length  Massless  Leg)  NEWTON-EULEF 


Figure  34:  Two  Link  Pendulum  Recovering  the  Negative  Orientations  (Newton-Euler) 


Two  Link  Inverted  Pendulum  (Constant  Length  Massless  Leg)  NEWTON-EULEF 


Figure  35:  Two  Link  Inverted  Pendulum  In  Steady  State  (Newton-Euler) 
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Body  Attitude  (theta-2)  NEWTON-EULER 


Figure  36:  Body  Attitude  Time  Response  of  the  Two  Link  Inverted  Pendulum  (Newton- 

Euler) 


Leg  Angle  (theta-1)  NEWTON-EULER 


Figure  37:  Leg  Angle  Response  of  the  Two  Link  Inverted  Pendulum  (Newton-Euler) 
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C.  RESULTS  OF  THE  DYNAMIC  SIMULATION 

Euler  integration  was  used  for  the  single  link  inverted  pendulum  simulation.  For  the 
two  link  inverted  pendulum  simulations,  Heun  integration  is  chosen.  Heun  is  a  more 
accurate  method  than  Euler,  because  it  converges  quadratically  as  the  step  size  is  decreased, 
while  Euler  integration  converges  only  linearly. 

As  seen  in  the  previous  section,  the  Newton-Euler  and  Lagrangian  solutions  of  the 
two  link  inverted  pendulum  problem  give  identical  results.  However,  the  running  times  of 
these  simulations  are  not  the  same.  The  time  needed  to  complete  the  Newton-Euler  version 
of  the  simulation  is  20  percent  longer  than  the  Lagrangian  version.  This  is  an  expected 
consequence  of  the  matrix  inversion  which  takes  place  in  the  Newton-Euler  version.  The 
Lagrangian  version  of  the  problem  computes  the  accelerations  without  matrix  inversion. 
However,  it  is  hard  to  derive  dynamic  equations  using  Lagrangian  methods  for  more 
complex  models  with  higher  degrees  of  freedom.  Moreover,  due  to  the  complexity  of  such 
equations,  it  is  suspected  that  Newton-Euler  models  for  postural  control  may  run  faster  for 
more  complex  systems.  This  will  certainly  be  true  if  O(n)  methods  are  used  [MCMI95]. 

D.  KINEMATIC  SIMULATION  OF  STEPPING  DYNAMAN 

1.    Stepping  Forward  Algorithm 

Figures  38  through  43  are  six  frames  from  the  kinematic  model  simulation.  These 
six  frames  show  one  step  cycle  of  Dynaman  during  forward  stepping. 
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Figure  38:  Forward  Stepping  (1) 


^^* 


Figure  39:  Forward  Stepping  (2) 
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Figure  40:  Forward  Stepping  (3) 


Figure  41:  Forward  Stepping  (4) 
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Figure  42:  Forward  Stepping  (5) 


Figure  43:  Forward  Stepping  (6) 
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2.    Stepping  Upward  Algorithm 

Figure  44  through  48  are  five  frames  from  the  kinematic  model  simulation.  These 
five  frames  show  one  step  cycle  of  the  Dynaman  while  stepping  upward. 


Figure  44:  Upward  Stepping  (1) 
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Figure  45:  Upward  Stepping  (2) 


Figure  46:  Upward  Stepping  (3) 
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Figure  47:  Upward  Stepping  (4) 


Figure  48:  Upward  Stepping  (5) 
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VI.  SUMMARY  AND  CONCLUSIONS 

A.      SUMMARY 

The  increasing  demand  for  realistic  3D  Virtual  Environments  requires  more 
research  on  modeling  human  motion.  There  exist  two  main  approaches  to  this  problem: 
kinematic  and  dynamic  modeling.  A  detailed  review  of  previous  work  on  kinematic 
modeling,  dynamic  modeling  and  control  of  graphical  representation  of  human  body 
motion  is  presented  in  this  thesis.  Kinematics  uses  only  the  geometric  constraints  of  the 
model  which  results  in  less  complicated  models.  However,  for  some  situations,  kinematic 
modeling  may  not  be  sufficient  to  simulate  human  motion  realistically.  Dynamics 
introduces  additional  properties  of  human  limb  segments  to  the  simulation,  such  as  mass 
and  moment  of  inertia.  These  additional  properties  allow  a  more  realistic  simulation.  The 
cost  of  this  realism  is  an  increase  in  computational  complexity.  The  goal  of  the  research  of 
this  thesis  is  to  develop  a  realistic  looking  human  model,  while  considering  the  complexity 
of  the  simulation  according  to  the  response  time  based  on  the  computational  power  of 
current  graphics  hardware. 

To  be  able  to  create  a  more  realistic,  real  time,  and  computationally  efficient  human 
model,  a  simple  dynamic  model  which  was  proposed,  but  not  fully  developed  in 
[MCGH79]  was  implemented.  The  model  is  a  two  link  planar  inverted  pendulum  which  is 
made  of  a  rigid  body  with  mass  and  a  supporting  massless  leg.  Two  different  methods, 
generalized  coordinates  with  Lagrangian,  and  the  Newton-Euler,  were  used  to  derive  the 
dynamic  equations  of  the  simulations.  The  implementation  results  verified  models  against 
each  other  and  related  models  in  the  literature  [GUBI74].  The  problem  of  determining  the 
gain  parameters  for  limb  segment  control  torques  is  also  investigated.  Since  the  degree  of 
the  characteristic  equation  is  four,  an  experimental  solution  is  presented,  instead  of  an 
analytic  one.  Another  result  is  the  difference  between  the  running  times  of  the  simulations. 


77 


The  simulation  which  uses  the  Newton-Euler  method  takes  20%  longer  than  the 
Lagrangian  version,  because  of  matrix  inversions.  It  is  also  observed  that  the  Lagrangian 
differential  equations  are  simpler  to  use  in  deriving  gain  values  for  the  system  in 
comparison  to  the  Newton-Euler  version  of  the  problem.  However,  the  Lagrangian  method 
would  be  hard  to  use  in  a  more  complex  model  with  higher  degrees  of  freedom.  For  such 
cases,  the  recursive  Articulated  Body  algorithm  [MCMI95]  which  has  O(n)  complexity 
needs  to  be  used. 

On  the  other  hand,  a  detailed  kinematic  human  model  which  contains  14  body  parts 
was  also  developed.  The  simulation  is  based  on  joints  angle  instead  of  the  Euler  angles 

which  used  in  previous  work.  IRIS  Performer      API,  which  interfaces  to  both  IRIS  GL™ 

and  OpenGL.  IRIS  Performer  and  provides  a  hierarchy  among  Dynamic  Coordinate 
Systems  (DCS),  was  used  to  implement  articulated  chain  structures  easily. 

B.      CONCLUSION  AND  FUTURE  RESEARCH 

This  thesis  covers  the  basics  for  simplified  dynamic  human  motion  simulation.  The 
model  which  is  introduced  in  this  thesis  has  only  the  torso  with  mass.  An  advanced  version 
of  this  simulation  may  consider  leg  mass  as  well.  The  existing  model  is  planar.  Another  step 
can  be  to  investigate  a  3D  version  of  the  model.  The  "Rigid-body"  class  could  continue  to 
be  used  for  this  purpose  since  provides  support  for  simulation  of  body  parts  in  3D. 
Multilink  more  complex  dynamic  models  also  need  to  be  investigated  by  considering 
different  solution  methods  such  as  Newton-Euler,  iterative  use  of  inverse  dynamics 
[KOOZ83],  and  the  Articulated  Body  Algorithm  [MCMI95].  Accuracy  and  time  response 
of  the  system  should  be  taken  into  account,  while  the  complexity  of  the  model  is  increased. 

Another  issue  is  to  integrate  the  existing  kinematic  and  dynamic  models.  This 
integration  may  result  a  more  realistic  human  model  than  the  developed  kinematic  model. 
It  will  be  computationally  cheaper  than  a  14  link  human  dynamic  model. 
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This  thesis  investigated  posture  control  of  a  human  model.  Stepping  is  another 
important  subject  in  human  motion  simulation.  An  efficient  automatic  stepping  algorithm 
needs  to  be  to  investigated  for  a  dynamically  simulated  human  model.  Active  (dynamic) 
balancing  should  be  considered  while  studying  automatic  stepping  control.  Advance 
features  in  further  research  in  stepping  is  speed  and  direction  control,  and  rough  terrain 
locomotion  for  the  walking  human  model.  For  direction  and  speed  control 
implementations,  user-computer  interaction  will  become  important.  A  speech  recognition 
interface  could  be  taken  in  the  consideration  as  a  realistic  solution  [DEVI96]. 

VRML  is  a  fast  growing  3D  modeling  language.  It  is  possible  to  execute  the  needed 
computation  by  using  Java  Classes  which  are  integrated  to  VRML  nodes.  Implementing  the 
simulations  by  using  VRML  and  Java  Scripting  would  provide  platform  independency 
which  would  allow  the  simulations  runable  in  other  than  graphic  workstation 
environments.  The  author  hopes  that  the  work  of  this  thesis  will  provide  a  foundation  for 
continuing  research  in  some  of  the  above  topics. 
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APPENDIX  A:  DYNAMIC  SIMULATION  SOFTWARE 

COMMON  FILES  IN  ALL  DYNAMIC  SIMULATIONS 

********************************************************************* 

File         :  robot-kinematics . cl 

Author       :  Dr.  R.  McGhee 

:  Naval  Postgraduate  School 

:  Monterey,  CA  93943 

Summary      :  This  file  contains  various  matrix  and  vector  operation 

:  functions. 
********************************************************************* 

(defun  transpose  (matrix)  ;A  matrix  is  a  list  of  row  vectors, 

(cond  ((null  (cdr  matrix))  (mapcar  'list  (car  matrix))) 

(t  (mapcar  'cons  (car  matrix)  (transpose  (cdr  matrix)))))) 

(defun  dot-product  (vector-1  vector-2)   ;A  vector  is  a  list  of  numerical  atoms 
(apply  '+  (mapcar  '*  vector-1  vector-2))) 

(defun  vector-magnitude  (vector)  (sqrt  (dot-product  vector  vector) ) ) 

(defun  post-multiply  (matrix  vector) 

(cond  ((null  (rest  matrix))  (list  (dot-product  (first  matrix)  vector))) 
(t  (cons  (dot-product  (first  matrix)  vector) 
(post-multiply  (rest  matrix)  vector) ) ) ) ) 

(defun  pre-multiply  (vector  matrix) 

(post-multiply  (transpose  matrix)  vector)) 

(defun  matrix-multiply  (A  B)       ;A  and  B  are  conformable  matrices, 
(cond  ((null  (cdr  A))  (list  (pre-multiply  (car  A)  B) ) ) 

(t  (cons  (pre-multiply  (car  A)  B)  (matrix-multiply  (cdr  A)  B) ) ) ) ) 

(defun  chain-multiply  (L)         ;L  is  a  list  of  names  of  conformable  matrices, 
(cond  ( (null  (cddr  L) )  (matrix-multiply  (eval  (car  L) )  (eval  (cadr  L) ) ) ) 

(t  (matrix-multiply  (eval  (car  L) )  (chain-multiply  (cdr  L)))))) 

(defun  cycle-left  (matrix)  (mapcar  'row-cycle-left  matrix)) 

(defun  row-cycle-left  (row)  (append  (cdr  row)  (list  (car  row)))) 

(defun  cycle-up  (matrix)  (append  (cdr  matrix)  (list  (car  matrix) ) ) ) 

(defun  unit-vector  (one-column  length)  ; Column  count  starts  at  1. 

(do  ( (n  length  (1-  n) ) 

(vector  nil  (cons  (cond  ((=  one-column  n)  1)  (t  0))  vector))) 
( (zerop  n)  vector) ) ) 


(defun  unit-matrix  (size) 

(do  ((row-number  size  (1-  row-number)) 
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(I  nil  (cons  (unit-vector  row-number  size)  I)}) 
((zerop  row-number)  I))) 

(defun  concat-matrix  (A  B)    ;A  and  B  are  matrices  with  equal  number  of  rows, 
(cond  ( (null  A)  B) 

(t  (cons  (append  (car  A)  (car  B) )  (concat-matrix  (cdr  A) 

(cdr  B) ) ) ) ) ) 

(defun  augment  (matrix) 

(concat-matrix  matrix  (unit-matrix  (length  matrix)))) 

(defun  normalize-row  (row)  (scalar-multiply  (/  1.0  (car  row))  row)) 

(defun  scalar-multiply  (scalar  vector) 
(cond  ((null  vector)  nil) 

(t  (cons  (*  scalar  (car  vector)) 

(scalar-multiply  scalar  (cdr  vector) ) ) ) ) ) 

(defun  solve-f irst-column  (matrix)         ,-Reduces  first  column  to  (10  ...  0). 
(do*  ( (remaining-row-list  matrix  (rest  remaining-row-list) ) 
(first-row  (normalize-row  (first  matrix))) 
(answer  (list  first-row) 

(cons  (vector-add  (first  remaining-row-list) 

(scalar-multiply  (-  (caar  remaining-row-list)) 
first-row) ) 
answer) ) ) 
( (null  (rest  remaining-row-list) )  (reverse  answer) ) ) ) 

(defun  vector-add  (vector-1  vector-2)  (mapcar  '+  vector-1  vector-2)) 

(defun  vector-subtract  (vector-1  vector-2)  (mapcar  '-  vector-1  vector-2)) 

(defun  first-square  (matrix)   ; Returns  leftmost  square  matrix  from  argument, 
(do  ((size  (length  matrix)) 

(remainder  matrix  (rest  remainder) ) 

(answer  nil  (cons  (firstn  size  (first  remainder))  answer))) 
( (null  remainder)  (reverse  answer) ) ) ) 

(defun  firstn  (n  list) 
(cond  ((zerop  n)  nil) 

(t  (cons  (first  list)  (firstn  (1-  n)  (rest  list)))))) 

(defun  max-car-f irstn  (n  list) 

(append  (max-car-f irst  (firstn  n  list))  (nthcdr  n  list))) 

(defun  matrix- inverse  (M) 

(do  ( (Ml  (max-car-f irst  (augment  M) ) 

(cond  ((null  Ml)  nil)        /Abort  for  singular  matrix. 

(t  (max-car-f irstn  n  (cycle-left  (cycle-up  Ml)))))) 
(n  (1-  (length  M) )  (1-  n) ) ) 
((or  (minusp  n)  (null  Ml))  (cond  ((null  Ml)  nil)  (t  (first-square  Ml)))) 
(setq  Ml  (cond  ((zerop  (caar  Ml))  nil)  (t  (solve-f irst-column  Ml)))))) 

(defun  max-car-f irst  (L)    ;L  is  a  list  of  lists.  This  function  finds  list  with 
(cond  ((null  (cdr  L) )  L)  /largest  car  and  moves  it  to  head  of  list  of  lists. 
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(t  (if  (>  (abs  (caar  L) )  (abs  (caar  (max-car-f irst  (cdr  L) ) ) ) )  L 
(append  (max-car-f irst  (cdr  L) )  (list  (car  L) ) ) ) ) ) ) 

(defun  dh-matrix  (cosrotate  sinrotate  costwist  sintwist  length  translate) 
(list  (list  cosrotate  (-  (*  costwist  sinrotate)) 

(*  sintwist  sinrotate)  (*  length  cosrotate)) 
(list  sinrotate  (*  costwist  cosrotate) 

(-  (*  sintwist  cosrotate))  (*  length  sinrotate)) 
(list  0.  sintwist  costwist  translate)  (list  0.  0.  0.  1.))) 

(defun  homogeneous -trans form  (azimuth  elevation  roll  x  y  z) 

(let  ( (spsi  (sin  azimuth))  (cpsi  (cos  azimuth))  (sth  (sin  elevation)) 
(cth  (cos  elevation))  (sphi  (sin  roll))  (cphi  (cos  roll))) 
(list  (list  (*  cpsi  cth)  (-  (*  cpsi  sth  sphi)  (*  spsi  cphi)) 
(+  (*  cpsi  sth  cphi)  (*  spsi  sphi))  x) 

(list  (*  spsi  cth)  (+  (*  cpsi  cphi)  (*  spsi  sth  sphi)) 
(-  (*  spsi  sth  cphi)  (*  cpsi  sphi))  y) 
(list  (-  sth)  (*  cth  sphi)  (*  cth  cphi)  z) 
(list  0.  0.  0.  1.  ) )  )  ) 

(defun  inverse-H  (H)  ;H  is  a  4x4  homogeneous  transformation  matrix, 

(let*  ( (minus-P  (list  (-  (fourth  (first  H) ) ) 

(-  (fourth  (second  H) ) ) 
(-  (fourth  (third  H) ) ) ) ) 
(inverse-R  (transpose  (first-square  (reverse  (rest  (reverse  H) ) ) ) ) ) 
(inverse-P  (post-multiply  inverse-R  minus-P))) 
(append  (concat-matrix  inverse-R  (transpose  (list  inverse-P))) 
(list  (list  0  0  0  1) ) ) ) ) 

(defun  rotation-matrix  (azimuth  elevation  roll) 

(let  ( (spsi  (sin  azimuth) )  (cpsi  (cos  azimuth) )  (sth  (sin  elevation) ) 
(cth  (cos  elevation))  (sphi  (sin  roll))  (cphi  (cos  roll))) 
(list  (list  (*  cpsi  cth)  (-  (*  cpsi  sth  sphi)  (*  spsi  cphi)) 
(+  (*  cpsi  sth  cphi)  (*  spsi  sphi))) 

(list  (*  spsi  cth)  (+  (*  cpsi  cphi)  (*  spsi  sth  sphi)) 
(-  (*  spsi  sth  cphi)  (*  cpsi  sphi))) 
(list  (-  sth)  (*  cth  sphi)  (*  cth  cphi))))) 

(defun  body-rate-to-euler-rate-matrix  (azimuth  elevation  roll) 

(let  ((sth  (sin  elevation))  (cth  (cos  elevation))  (tth  (tan  elevation)) 
(sphi  (sin  roll))  (cphi  (cos  roll))) 
(list  (list  1  (*  tth  sphi)  (*  tth  cphi)) 
(list  0  cphi  (-  sphi) ) 
(list  0  (/  sphi  cth)  (/  cphi  cth))))) 
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•••♦•••••A*********************************************************** 

File         :  harmonic-equation. cl 
Author       :  Dr.  R.  McGhee 

Naval  Postgraduate  School 

Monterey,  CA  93  943 

Summary      :  This  file  contains  functions  to  create  a  window  and  execute 

drawing  operations  on  the  window, 
•••••a*************************************************************** 

(require  :xcw) 

(cw: initialize- common-windows) 


(defun  make-window  () 
( cw : make-window- stream 


borders  5 

left  10 

bottom  280 

width  600 

height  600 

title  "Harmonic  Equation" 

activate-p  t)  )   ,-Make  window  visible, 


(defun  scale-point-coordinates  (x-y-list  enlargement-factor) 
(let  ( (x  (first  x-y-list))  (y  (second  x-y-list))) 
(list  (+  50  (round  (*  enlargement- factor  x) ) ) 

(+  225  (round  (*  enlargement- factor  y) ) ) ) ) ) 

(defun  draw-coordinate-axes  (window) 

(cw:draw-line  window  (cw:make-position  :x  20  :y  225) 

(cwrmake-position  :x  570  :y  225) 

: brush-width  2) 
(cw: draw- line  window  (cw: make-position  :x  5  0  :y  20) 

(cw:make-position  :x  50  :y  560) 

: brush-width  2)) 

(defun  draw-line-in-window  (window  enlargement- factor  start  end) 

(let  ((scaled-start  (scale-point-coordinates  start  enlargement- factor ) ) 
(scaled-end  (scale-point-coordinates  end  enlargement-factor) ) ) 
(cw: draw- line  window 

(cw:make-position  :x  (first  scaled-start)  :y  (second  scaled-start)) 
(cw:make-position  :x  (first  scaled-end)  :y  (second  scaled-end))))) 
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SINGLE  LINK  INVERTED  PENDULUM  SIMULATION  WITH  NEWTON-EULER 

********************************************************************* 

File         :  euler-angle-rigid-body . cl 
Author       :  Dr.  R.  McGhee 

Naval  Postgraduate  School 

Monterey,  CA  93943 
Modified  by  :  Mehmet  Bediz 
Summary      :  This  file  contains  rigid  body  class  which  is  implemented 

by  Dr.  R.  McGhee.  The  function  "test-rigid-body-f orces-and- 

torques-three-with-M"  and  the  other  functions  called  by  this 

function  are  implemented  by  Mehmet  Bediz. 

r************************************************************** 

(defclass  rigid-body 

() 
((posture  ;The  vector  (xe  ye  ze  phi  theta  psi) . 

: initform  '(000000) 

:initarg  : posture 

: accessor  posture) 
(posture-rate  ;The  vector  (xe-dot  ye-dot  ze-dot  phi-dot  theta-dot  psi-dot) . 

: initarg  : posture-rate 

: accessor  posture-rate) 
(velocity  ;The  six-vector  (u  v  w  p  q  r)  in  body  coordinates. 

: initform  '(000   000) 

: initarg  : velocity 

: accessor  velocity) 
(velocity-growth-rate   ;The  vector  (u-dot  v-dot  w-dot  p-dot  q-dot  r-dot) . 

: accessor  velocity-growth-rate) 
(forces-and- torques     ;The  vector  (Fx  Fy  Fz  L  M  N)  in  body  coordinates. 

;:initform  (list  0  0  (-  *gravity*)  0  0  0) 

rinitform  (list  0  0  0  0  0  0) 

: accessor  forces-and- torques) 
(moments-of-inertia     ;The  vector  ( Ix  Iy  Iz)  in  principal  axis  coordinates. 

: initform  ' (1  900  1) 

: initarg  :moments-of-inertia 

: accessor  moments-of-inertia) 
(mass 

: initform  100 

: initarg  :mass 

: accessor  mass) 
(node-list    ; (x  y  z  1)  in  body  coord  for  each  node.  Starts  with  (0001). 

:initform  '((0  0  0  1)  (-0.5  -0.5  0.5  1)  (0.5  -0.5  0.5  1)(0.5  -0.5  -0.5  1) 

(-0.5  -0.5  -0.5  1H-0.5  0.5  0.5  1)(0.5   0.5  0.5  1) 
(0.5   0.5  -0.5  l)(-0.5   0.5  -0.5  1)(0  0  0  1) 
(0031)) 

:  initarg  :node-list 

:accessor  node-list) 
(polygon-list 

rinitform  ' ( (1  5  8  4 ) ( 5  6  7  8 ) (6  2  3  7) (2  1  4  3) (9  10)) 

: initarg  :polygon-list 

:accessor  polygon-list) 
(transformed-node-list   ; (x  y  z  1)  in  earth  coord  for  each  node  in  node-list, 

rinitform  '((0  0  0  1)  (-2  -2  0  1)  (2  -2  0  1)(2  -2  -30  l)(-2  -2  -30  1) 

(-2   2  0  1)  (2   2  0  1) (2   2  -30  l)(-2   2  -30  1)) 
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: accessor  transf ormed-node-list) 
(H-matrix 

:initform  (unit-matrix  4) 

: accessor  H-matrix) 
(time-stamp 

accessor  time-stamp))) 

(defmethod  initialize  ( (body  rigid-body) ) 

(setf  (transf ormed-node-list  body)  (node-list  body) ) 

(setf  (velocity-growth-rate  body)  (update-velocity-growth-rate  body) ) 

(setf  (posture-rate  body)  (earth-velocity  body) ) 

(setf  (time-stamp  body)  (get-internal-real-time)))) 

(defmethod  set-transf ormed-node-list-z ( (body  rigid-body)  z) 
(setf  (third ( fourth  (transf ormed-node-list  body)))(-  z) ) 
(setf  (third(fifth  (transf ormed-node-list  body)))  (-  z) ) 
(setf  (third (eighth  (transf ormed-node-list  body)))  (-  z) ) 
(setf  (third(ninth  (transf ormed-node-list  body)))  (-  z)) 
(transf ormed-node-list  body))) 

(defun  transform  (obj) 

(list  0  (first  obj) (second  obj) (third  obj))) 

(defun  reverse- trans form  (obj) 

(list  (second  obj) (third  obj) (fourth  obj)  1)) 

(defmethod  move  ( (body  rigid-body)  azimuth  elevation  roll  x  y  z) 
(setf  (posture  body)  (list  x  y  z  roll  elevation  azimuth)) 
(setf  (H-matrix  body) 

( homogeneous - trans  form  azimuth  elevation  roll  x  y  z) ) 
(transform-node-list  body)) 

(defmethod  get-delta-t  ((body  rigid-body))  0.02) 
(let*  ( (new- time  (get-internal-real-time)) 

(delta-t  (/  (-  new-time  (time-stamp  body))  1000))) 
(setf  (time-stamp  body)  new-time) 
delca-t) ) 

(defmethod  update-rigid-body  ((body  rigid-body))      ;Euler  integration, 
(let*  ((delta-t  (get-delta-t  body))) 

(update-posture  body  delta-t) ;  EULER 

(update-velocity  body  delta-t) ;  EULER 

(setf  (H-matrix  body)  (homogeneous-transform  (sixth  (posture  body) ) 

(fifth  (posture  body))  (fourth  (posture  body))  (first  (posture  body)) 
(second  (posture  body))  (third  (posture  body)))) 
(transform-node-list  body) 
(update-velocity-growth-rate  body) ) ) 

(defmethod  update-velocity-growth-rate  ( (body  rigid-body) ) 

(setf  (velocity-growth-rate  body)  ; Assumes  principal  axis  coordinates  with 
(multiple-value-bind  ; origin  at  center  of  gravity  of  body. 

(Fx  Fy  Fz  L  M  N    u  v  w  p  q  r    Ix  Iy  Iz)  ; Declares  local  variables, 
(values-list  /Values  assigned. 

( append 

( f orces-and-torques  body)  (velocity  body)  (moments-of-inertia  body))) 
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(list 

(+    ( 

(+    ( 

(  +    ( 

(/     ( 

(/     ( 

(/     ( 

v  r)  (*  -1  w  q)  ( /  Fx  (mass  body)) 

♦gravity*  (first  (third  (H-matrix  body))))) 

w  p)  (*  -1  u  r)  (/  Fy  (mass  body)) 

♦gravity*  (second  (third  (H-matrix  body))))) 

u  q)  (*  -1  v  p)  ( /  Fz  (mass  body)) 

♦gravity*  (third  (third  (H-matrix  body))))) 

(*  (-  Iy  Iz)  q  r)  L)  Ix) 

(*  (-  Iz  Ix)  r  p)  M)  Iy) 

(*  (-  Ix  Iy)  p  q)  N)  Iz)))))   ;Value  returned. 

(defmethod  update-velocity  ((body  rigid-body)  delta-t)  ;Euler  integration, 
(setf  (velocity  body) 

(vector-add  (velocity  body) 

(scalar-multiply  delta-t  (velocity-growth-rate  body) ) ) ) ) 

(defmethod  update-posture  ((body  rigid-body)  delta-t)  ;Euler  integration, 
(setf  (posture-rate  body)  (earth-velocity  body) ) 
(setf  (posture  body) 

(vector-add  (posture  body)  (scalar-multiply  delta-t  (posture-rate  body) ) ) ) ) 

(defmethod  transform-node-list  ( (body  rigid-body) ) 
(setf  ( transf ormed-node-list  body) 
(mapcar  #' (lambda  (node-location) 

(post-multiply  (H-matrix  body)  node-location) ) 
(node-list  body) ) ) ) 

(defconstant  *gravity*  32.2185) 

(defmethod  earth-velocity  ( (body  rigid-body) ) 

(let*  ((linear-velocity  (firstn  3  (velocity  body))) 
(rotational-velocity  (cdddr  (velocity  body) ) ) 
(posture  (posture  body) ) 
(R-matrix  (rotation-matrix  (sixth  posture)  (fifth  posture) 

(fourth  posture) ) ) 
(linear-earth-velocity  (post-multiply  R-matrix  linear-velocity)) 
(T-matrix  (body-rate-to-euler-rate-matrix  (sixth  posture) 

(fifth  posture)  (fourth  posture))) 
(rotational-earth-velocity  (post-multiply  T-matrix 

rotational-velocity) ) ) 
(append  linear-earth-velocity  rotational-earth-velocity) ) ) 

(defun  test-rigid-body  () 

(setf  airplane-1  (make-instance  'rigid-body)) 

(initialize  airplane-1) 

(setf  camera-1  (make- instance  'strobe-camera)) 

(move  camera-1  (-  (/  pi  2))  0   0  0  30  0  ) 

(take-picture  camera-1  airplane-1) 

(dotimes  (i  20  'done)  (update-rigid-body  airplane-1) ) 

(take-picture  camera-1  airplane-1) ) 
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-  Inverted  Pendulum  (fixed  leg  length) 

-  state  vector  x  =  (theta  theta-dot) 

-  state  vector  u  =  (M) 

-  control  torque  at  the  pivot  (M) 

-  Newton-Euler  Method 

3x3  matrix  inversion  to  calculate  forces  and  moments 
RIGID  Body  class  to  compute  accelerations 

-  Heun  Integration 


(defmethod  forces-earth-to-body  (Fx-e  Fy-e  Fz-e  theta) 
(let*  ((forces-earth  (list  Fx-e  Fy-e  Fz-e)) 

(R-matrix  (rotation-matrix  0  (-  theta)  0))  ;Since  planar 
(forces-body  (post-multiply  R-matrix  forces-earth))) 

forces-body) ) 

(defmethod  update-forces-and-torques-three-with-M  ( (body  rigid-body)  M) 
(let*   ( (equations-solution-vector  (solve-three-equation-system-with-M 

body  M) ) 
(Fx-earth  (second  equations-solution-vector) ) 
(Fz-earth  (third  equations-solution-vector)) 
(F-body   (forces-earth-to-body  Fx-earth  0  Fz-earth 

(fifth  (posture  body))))) 
(setf  f orces-and-torques  (list  (first  F-body) 

0 

(third  F-body) 
0 

(*  (first  equations-solution-vector) 
(second  (moments-of-inertia  body) ) ) 
0)  )  )  ) 

(defun  solve-three-equation-system-with-M  (body  M) 

(setf  m       (mass  body) ) 

(setf  1        3) 

(setf  I        (second  (moments-of-inertia  body) ) ) 

(setf  theta    (fifth   (posture   body))) 

(setf  theta-dot    (fifth   (velocity  body))) 

(setf  g     *gravity*) 

(post-multiply  (matrix- inverse  (list  (list  I  (*  -1  1  (cos  theta)) 

(*  1  (sin  theta) ) ) 
(list  (*  m   1  (cos  theta))     1       0  ) 
(list  (*  m  1  (sin  theta)  )    0    -1  )  )  ) 
(list   M 

(*  (sqr  theta-dot)  m  1  (sin  theta)) 

(-  (*  m  g)  (*  (sqr  theta-dot)  m  1  (cos  theta)))))) 
(defun  compute-M  (body  K- theta  K- theta-dot) 
(setf   theta    (fifth   (posture   body))) 
(setf   theta-dot    (fifth   (velocity  body))) 


(+  (*  -1  K-theta  theta)  (*  -1  K-theta-dot   theta-dot) ) ) 

(defun  test-rigid-body-f orces-and-torques-three-with-M  (K-theta  K-theta-dot) 
(setf  inverted-pendulum  (make-instance  'rigid-body)) 
(initialize  inverted-pendulum) 

(setf  camera-1  (make-instance  'strobe-camera)) 

(move  camera-1  (deg-to-rad  (-  90) ) (deg-to-rad  0) (deg-to-rad  0)  0  10  0) 
(move  inverted-pendulum  0-10000) 
;( take-picture  camera-1  inverted-pendulum) 

(setf  graph- 1 -window  (make-window-graph- 1) ) 
(draw-coordinate-axes  graph- 1-window) 

(setf  old-theta  0.5) 
(do  (  (i  0  (+  i  1)  )  ) 

( (  >  i  1000)   'end) 

(setf  (forces-and- torques  inverted-pendulum) 

( update- f orces-and-torques-three-with-M  inverted-pendulum 
(compute-M  inverted-pendulum  K-theta  K-theta-dot))) 
(update- rig id-body  inverted-pendulum) 
(cw:clear  (camera-window  camera-1)) 
(take-picture  camera-1  inverted-pendulum) 
(draw-line-in-window  graph- 1-window  80 

(list 

(*   (get-delta-t  inverted-pendulum)  (-  i  1)) 
(*  -1   old-theta) ) 
(list 

(*    (get-delta-t  inverted-pendulum)  i) 

(*  -1  (fifth  (posture  inverted-pendulum))))) 

(setf  old-theta  (fifth  (posture  inverted-pendulum))))) 

(defun  sqr(x) 
(*  xx)) 

(defun  make-window-graph-1  () 


( cw : make-window-s  tream 


borders  5 

left  0 

bottom  550 

width  450 

height  450 

title  "Body  Attitude    (theta)    NEWTON- EULER" 

activate-p  t) )   ;Make  window  visible. 
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********************************************************************* 


File 
Author 


Summary 


load-euler- files . cl 

Mehmet  Bediz 

Naval  Postgraduate  School 

Monterey,  CA  93  943 

This  file  contains  functions  to  load  files  and  to  test 

the  simulations. 
********************************************************************* 


Load  files 


(load  "harmonic-equation. cl " ) 

(load  "robot-kinematics . cl " ) 

( load  " euler-angle-rigid-body . cl " ) 

(load  "strobe-camera . cl " ) 


Tests 


(defun  graph_l  () 

(solve-robot-attitude  0.01  1.0  10000  2000  109660  20000)) 

(defun  graph_2  () 

(solve-robot-altitude-z_zO  0.01  1.0  10000  2000  109660  20000)) 

(defun  graph_3  () 

(solve-robot-altitude-z  0.01  1.0  10000  2000  109660  20000)) 

(defun  draw-converted-pendulum-test-real-parameters  () 

(draw-converted-pendulum  0.01  2.0  10000  2000  109660  20000)) 

(defun  draw-converted-pendulum-test-zero-gain  () 

(draw-converted-pendulum  0.01  2. 00       0      0      0)) 
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****  +  **  +  **  +  ***  +  ***********■***********•******•****  +  +  **■*********•***•**■***  + 

File         :  strobe-camera . cl 
Author       :  Dr.  R.  McGhee 

Naval  Postgraduate  School 

Monterey,  CA  93943 

Summary      :  This  file  contains  strobe-camera  class  definitions. 
********************************************************************* 

(require  :xcw) 

(cw: initialize-common-windows) 

(defclass  strobe-camera  (rigid-body) 
( (focal-length 

: accessor  focal-length 
: initf orm  6) 
( camera-window 
: accessor  camera-window 

: initf orm  (cw: make-window- stream  : borders  5 

:left  500 
rbottom  500 
:width  650 
rheight  650 

: title  "strobe-camera-image" 
:activate-p  t) ) 
(H-matrix 

:initform  (homogeneous -trans form  .3  -.3  0  -300  -90  -90)) 
( inverse-H-matrix 
: accessor  inverse-H-matrix 

:initform  (inverse-H  (homogeneous-transform  .3  -.3  0  -300  -90  -90))) 
( enlargement- factor 
: accessor  enlargement- factor 
:initform  30) ) ) 

(defmethod  move  ((camera  strobe-camera)  azimuth  elevation  roll  x  y  z) 

(setf  (H-matrix  camera)  (homogeneous-transform  azimuth  elevation  roll  x  y  z) ) 
(setf  (inverse-H-matrix  camera)  (inverse-H  (H-matrix  camera)))) 

(defmethod  take-picture  ( (camera  strobe-camera)  (body  rigid-body) ) 
(let  ((camera-space-node-list  (mapcar  #' (lambda  (node-location) 
(post-multiply  (inverse-H-matrix  camera)  node-location)) 
( transf ormed-node-list  body)))) 
(dolist  (polygon  (polygon-list  body) ) 

(clip-and-draw-polygon  camera  polygon  camera-space-node-list) ) ) ) 

(defmethod  clip-and-draw-polygon 

( (camera  strobe-camera)  polygon  node-coord-list) 
(do*  ((initial-point  (nth  (first  polygon)  node-coord-list)) 
(from-point  initial-point  to-point) 

(remaining-nodes  (rest  polygon)  (rest  remaining-nodes) ) 
(to-point  (nth  (first  remaining-nodes)  node-coord-list) 
(if  (not  (null  (first  remaining-nodes))) 

(nth  (first  remaining-nodes)  node-coord-list)))) 
( (null  to-point) 

(draw-clipped-projection  camera  from-point  initial-point) ) 
(draw-clipped-projection  camera  from-point  to-point) ) ) 


91 


(defmethod  draw-clipped-projection  ( (camera  strobe- camera)  from-point  to-point) 
(cond  ((and  (<=  (first  from-point)  (focal-length  camera)) 

(<=  (first  to-point)  (focal-length  camera)))  nil) 
( (<=  (first  from-point)  (focal-length  camera)) 
(draw-line-in-camera-window  camera 

(perspective-transform  camera  (from-clip  camera  from-point  to-point)) 
(perspective-transform  camera  to-point) ) ) 
( (<=  (first  to-point)  (focal-length  camera)) 
(draw-line-in-camera-window  camera 

(perspective- transform  camera  from-point) 

(perspective-transform  camera  (to-clip  camera  from-point  to-point) ) ) ) 
(t  (draw-line-in-camera-window  camera 

(perspective- transform  camera  from-point) 
(perspective-transform  camera  to-point) ) ) ) ) 

(defmethod  from-clip  ( (camera  strobe-camera)  from-point  to-point) 
(let  ((scale-factor  (/  (-  (focal-length  camera)  (first  from-point)) 

(-  (first  to-point)  (first  from-point))))) 
(list  (+  (first  from-point) 

(*  scale-factor  (-  (first  to-point)  (first  from-point)))) 
(+  (second  from-point) 

(*  scale-factor  (-  (second  to-point)  (second  from-point)))) 
(+  (third  from-point) 

(*  scale-factor  (-  (third  to-point)  (third  from-point))))  1))) 

(defmethod  to-clip  ( (camera  strobe-camera)  from-point  to-point) 
(from-clip  camera  to-point  from-point)) 

(defmethod  draw-line-in-camera-window  ( (camera  strobe- camera)  start  end) 
(cw: draw- line  (camera-window  camera) 

(cw:make-position  :x  (+  150  (first  start))  :y  (+  150  (second 
start) ) ) 

(cw:make-position  :x  (+  150  (first  end))  :y  (+  150  (second 
end) ) ) 

: brush-width  0) ) 

(defmethod  perspective-transform  ( (camera  strobe- camera)  point-in-camera-space) 
(let*  ( (enlargement- factor  (enlargement- factor  camera) ) 
(focal-length  (focal-length  camera)) 

(x  (first  point-in-camera-space))   ;x  axis  is  along  optical  axis 
(y  (second  point-in-camera-space))  ,-y  is  out  right  side  of  camera 
(z  (third  point-in-camera-space)))  ,-z  is  out  bottom  of  camera 
(list  (+  (round  (*  enlargement- factor  (/  (*  focal-length  y)  x) ) ) 

150)         ; to  right  in  camera  window 

(+  150  (round  (*  enlargement-factor  (/  (*  focal-length  (-  z))  x) ) 

)  )  )  )  )   ,-up  in  camera  window 

(defun  test-camera  (z  theta)  ; Produces  top  view  of  default  rigid-body, 
(setf  robot-leg  (make-instance  'rigid-body)) 
(initialize   robot-leg) 

(set-transf ormed-node-list-z  robot-leg  z) 
(set-transf ormed-node-list-theta  robot-leg  theta) 
(setf  camera-1  (make-instance  'strobe-camera)) 
(move  camera-1  (deg-to-rad  0) (deg-to-rad  0) (deg-to-rad  0)  -30  0  0) 
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(take-picture  camera-1  robot-leg)) 
(defun  deg-to-rad  (angle)  (*  .01745329251994329  angle)) 
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TWO  LINK  INVERTED  PENDULUM  SIMULATION  WITH  LAGRANGIAN 

********************************************************************* 


File 
Author 


Modified  by- 
Summary 


euler-angle-rigid-body . cl 

Dr .  R .  McGhee 

Naval  Postgraduate  School 

Monterey,  CA  93  943 

Mehmet  Bediz 

This  file  contains  rigid  body  class  which  is  implemented 

by  Dr.  R.  McGhee.  The  function  "draw- inverted-pendulum" 

and  the  other  functions  called  by  this 

function  are  implemented  by  Mehmet  Bediz. 
••••••••A************************************************************ 

(defclass  rigid-body 

0 
((posture  ;The  vector  (xe  ye  ze  phi  theta  psi) . 

:initform  '(000000) 

rinitarg  :posture 

accessor  posture) 
(posture-rate  ;The  vector  (xe-dot  ye-dot  ze-dot  phi-dot  theta-dot  psi-dot) . 

:initarg  :posture-rate 

: accessor  posture-rate) 
(velocity  ;The  six-vector  (u  v  w  p  q  r)  in  body  coordinates. 

:initform  '(000   000) 

:initarg  : velocity 

:accessor  velocity) 
(velocity-growth-rate   ;The  vector  (u-dot  v-dot  w-dot  p-dot  q-dot  r-dot) . 

: accessor  velocity-growth-rate) 
(forces-and- torques     ;The  vector  (Fx  Fy  Fz  L  M  N)  in  body  coordinates. 

:initform  (list  0  0  0  0  0  0) 

:accessor  forces-and- torques) 
(moments-of-inertia     ;The  vector  (Ix  Iy  Iz)  in  principal  axis  coordinates. 

:initform  ' (1  100  1) 

rinitarg  :moments-of-inertia 

: accessor  moments-of-inertia) 
(mass 

:initform  100 

:initarg  :mass 

: accessor  mass) 
(node-list     ; (x  y  z  1)  in  body  coord  for  each  node.  Starts  with  (0  0  0  1) . 

:initform  '((0  0  0  1)  (-0.5  -0.5  0.5  1)  (0.5  -0.5  0.5  1)(0.5  -0.5  -0.5  1) 

(-0.5  -0.5  -0.5  1M-0.5   0.5  0.5  1)  (0.5   0.5  0.5  1) 
(0.5   0.5  -0.5  1M-0.5   0.5  -0.5  1)(0  0  0  1)(0  0  3  1)) 

:initarg  :node-list 

:accessor  node-list) 
(polygon-list 

:initform  ' ( ( 1  5  8  4) (5  6  7  8 ) (6  2  3  7 ) (2  1  4  3) (9  10)) 

:initarg  :polygon-list 

:accessor  polygon-list) 
( transf ormed-node-list   ; (x  y  z  1)  in  earth  coord  for  each  node  in  node-list. 

:initform  '((0001)  (-2  -2  0  1)  (2  -2  0  1) (2  -2  -30  l)(-2  -2  -30  1) 

(-2   2  0  1)  (2   2  0  1)  (2   2  -30  l)(-2   2  -30  D) 

: accessor  transf ormed-node-list) 
(H-matrix 
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:initform  (unit-matrix  4) 
: accessor  H-matrix) 
(time- stamp 
:accessor  time- stamp) ) ) 

(defmethod  initialize  ( (body  rigid-body) ) 

(setf  ( trans formed-node- list  body)  (node-list  body) ) 

(setf  (velocity-growth-rate  body)  (update-velocity-growth-rate  body) ) 

(setf  (posture-rate  body)  (earth-velocity  body) ) 

(setf  (time-stamp  body)  (get-internal-real-time)))) 

(defmethod  set-transf ormed-node-list-z ( (body  rigid-body)  z) 
(setf  (third(fourth  ( transf ormed-node-list  body)))(-  z)  ) 
(setf  (third(fifth  (transf ormed-node-list  body)))  (-  z)) 
(setf  (third(eighth  (transf ormed-node-list  body)))  (-  z)) 
(setf  (third(ninth  (transf ormed-node-list  body)))  (-  z) ) 
(transf ormed-node-list  body) ) ) 

(defun  transform  (obj) 

(list  0  (first  obj) (second  obj) (third  obj))) 

(defun  reverse- trans form  (obj) 

(list  (second  obj) (third  obj) (fourth  obj)  1)) 

(defmethod  move  ( (body  rigid-body)  azimuth  elevation  roll  x  y  z) 
(setf  (posture  body)  (list  x  y  z  roll  elevation  azimuth)) 
(setf  (H-matrix  body) 

(homogeneous-transform  azimuth  elevation  roll  x  y  z) ) 
(transform-node-list  body) ) 

(defmethod  get-delta-t  ((body  rigid-body))  0.005) 

(defmethod  forces-earth-to-body  (Fx-e  Fy-e  Fz-e  theta) 
(let*  ((forces-earth  (list  Fx-e  Fy-e  Fz-e)) 

(R-matrix  (rotation-matrix  0  (-  theta)  0)) 
(forces-body  (post-multiply  R-matrix  forces-earth))) 

forces-body) ) 

(defmethod  update-rigid-body  ( (body  rigid-body) )      ;Euler  integration, 
(let*  ( (delta-t  (get-delta-t  body))) 

(update-posture  body  delta-t) ;  EULER 

(update-velocity  body  delta-t) ;  EULER 

(setf  (H-matrix  body)  (homogeneous -trans form  (sixth  (posture  body) ) 

(fifth  (posture  body))  (fourth  (posture  body))  (first  (posture  body)) 
(second  (posture  body) )  (third  (posture  body) ) ) ) 
(transform-node-list  body) 

(update-velocity-growth-rate  body) ) ) 

(defmethod  update-velocity-growth-rate  ( (body  rigid-body) ) 

(setf  (velocity-growth-rate  body)  /Assumes  principal  axis  coordinates  with 
(multiple-value-bind  ; origin  at  center  of  gravity  of  body. 

(Fx  Fy  Fz  L  M  N   u  v  w  p  q  r    Ix  Iy  Iz)  ; Declares  local  variables. 
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(values-list  ;Values  assigned, 

(append 

( f orces -and- torques  body)  (velocity  body)  (moments-of-inertia  body))) 


(list  (  + 


(/ 
(/ 
(/ 


*  v  r)  (*  -1  w  q)  (/  Fx  (mass  body)) 

*  *gravity*  (first  (third  (H-matrix  body))))) 

*  w  p)  (*  -1  u  r)  (/  Fy  (mass  body)) 

*  *gravity*  (second  (third  (H-matrix  body))))) 

*  u  q)  (*  -1  v  p)  (/  Fz  (mass  body)) 

*  *gravity*  (third  (third  (H-matrix  body) ))) ) 
+  (*  (-  ly  Iz)  q  r)  L)  Ix) 

+  (*  (-  Iz  Ix)  r  p)  M)  ly) 

+  (*  (-  Ix  ly)  p  q)  N)  Iz)))))   ;Value  returned. 


(defmethod  update-velocity  ((body  rigid-body)  delta-t)  ;Euler  integration, 
(setf  (velocity  body) 

(vector-add  (velocity  body) 

(scalar-multiply  delta-t  (velocity-growth-rate  body) ) ) ) ) 

(defmethod  update-posture  ( (body  rigid-body)  delta-t)  ;Euler  integration, 
(setf  (posture-rate  body)  (earth-velocity  body) ) 
(setf  (posture  body) 

(vector-add  (posture  body)  (scalar-multiply  delta-t  (posture-rate  body))))) 

(defmethod  transform-node-list  ( (body  rigid-body) ) 
(setf  ( transformed-node-list  body) 
(mapcar  #' (lambda  (node-location) 

(post-multiply  (H-matrix  body)  node-location) ) 
(node-list  body) ) ) ) 

(def constant  *gravity*  32.2185) 

(defmethod  earth-velocity  ( (body  rigid-body) ) 

(let*  ((linear-velocity  (firstn  3  (velocity  body))) 
(rotational-velocity  (cdddr  (velocity  body) ) ) 
(posture  (posture  body) ) 
(R-matrix  (rotation-matrix  (sixth  posture)  (fifth  posture) 

(fourth  posture))) 
(linear-earth-velocity  (post-multiply  R-matrix  linear-velocity)) 
(T-matrix  (body-rate-to-euler-rate-matrix  (sixth  posture) 

(fifth  posture)  (fourth  posture))) 
(rotational-earth-velocity  (post-multiply  T-matrix 

rotational-velocity) ) ) 
(append  linear-earth-velocity  rotational-earth-velocity) ) ) 

(defun  test-rigid-body  () 

(setf  airplane-1  (make-instance  'rigid-body)) 

(initialize  airplane-1) 

(setf  camera-1  (make-instance  'strobe-camera)) 

(move  camera-1  (-  (/  pi  2))  0   0  0  30  0  ) 

(take-picture  camera-1  airplane-1) 

(dotimes  (i  20  'done)  (update-rigid-body  airplane-1) ) 

(take-picture  camera-1  airplane-1) ) 
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-  Two  Link  Inverted  Pendulum 

-  Generalized  Coordinates  with  Lagrangian  Method 

-  Massless  leg  and  a  single  rigid  body  (fixed  leg  length) 

-  State  vector  x  =  (theta-1  theta-1-dot  theta-2  theta-2-dot) 

-  State  vector  u  =  (M-l  M-2) 

-  control  torque  at  the  ankle  (M-l) 
;  -  control  torque  at  the  hip  (M-2) 

-  Heun  Integration 
;               -  By  Mehmet  Bediz 

(setf   1        0.5) 
(setf   r       3) 
(setf  time-step  0.01) 

(defun  euler-step-constant-leg  (state-vector  M-l  M-2) 
(mapcar  '+  state-vector  (scalar-multiply  time-step 

(derivative-state-vector 
state-vector  M-l  M-2) ) ) ) 

(defun  heun-step-constant-leg  (x  K-theta-1  K-theta-1-dot 

K-theta-2  K-theta-2-dot) 
(setf   M-l  (compute-M-1  x  K-theta-1  K-theta-1-dot 

K-theta-2  K-theta-2-dot) ) 
(setf   M-2  (compute-M-2  x  K-theta-1  K-theta-1-dot 

K-theta-2  K-theta-2-dot) ) 
(mapcar  ' +  x  (scalar-multiply  (*  time-step  .5) 
(vector-add  (derivative-state-vector  x  M-l  M-2) 
(derivative- state- vector 

(euler-step-constant-leg  x  M-l  M-2)  M-l  M-2))))) 

(defun  derivative-state-vector  (state-vector   M-l  M-2) 
(list  (second  state-vector) 

(compute-theta-1-double-dot  state-vector   M-l  M-2) 

(fourth  state-vector) 

(compute- theta-2-double-dot  state-vector   M-l  M-2))) 

(defun  compute-theta-1-double-dot  (state-vector  M-l  M-2)  ;  Eq  3.51 


(first  state-vector)) 
(second  state-vector) ) 
(third  state-vector) ) 
(fourth  state-vector)) 


(  /  (+  (*  llmmrg  theta-2) 

(*  -1  m  g  r  (+  I  (*  m  1  1) )  theta-1) 
(*  -1  (+  I  (*  m  1  1)  )  M-l) 
(*  (+  (*  1  m  r)  I  (*  m  1  1)  )  M-2) 
) 
(*  -1  I  m  r  r) ) ) 


(setf 

m 

100) 

(setf 

1 

0.5) 

(setf 

r 

3) 

(setf 

I 

100) 

(setf 

theta-1 

(setf 

theta-1- 

-dot 

(setf 

theta-2 

(setf 

theta-2- 

-dot 

(setf 

g     32 

.2) 
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(defun  compute- theta-2-double-dot  (state-vector  M-l  M-2)  ;  Eq  3.54 

(setf  m       100) 

(setf  1        0.5) 

(setf  r        3) 

(setf  I        100) 

(setf  theta-1        (first   state-vector)) 

(setf  theta-1-dot    (second  state-vector) ) 

(setf  theta-2        (third  state-vector) ) 

(setf  theta-2-dot    (fourth  state-vector)) 

(setf  g     32.2) 

(  /  (+  (*  m  g  1  r  theta-2) 

(*  -1  m  g  1  r  theta-1) 
(*  -1  1  M-l) 


(*  (+  r  1)  M-2) 


) 
r  I) 


) 


(defun  compute-M-1  (state-vector  K-theta-1  K-theta-1-dot 

K-theta-2  K-theta-2-dot) 
(setf  theta-1  (first  state-vector)) 
(setf  theta-1-dot  (second  state-vector) ) 
(+  (*  -1  K-theta-1  theta-1) (*  -1  K-theta-1-dot  theta-1-dot) ) ) 

(defun  compute-M-2  (state-vector  K-theta-1  K-theta-1-dot 

K-theta-2  K-theta-2-dot) 
(setf  theta-2  (third  state-vector)) 
(setf  theta-2-dot  (fourth  state-vector)) 
(+  (*  -1  K-theta-2  theta-2) (*  -1  K-theta-2-dot  theta-2-dot) ) ) 

(defun  compute-y  (state-vector) 

(setf  theta-1        (first   state-vector)) 

(setf  theta-1-dot    (second  state-vector) ) 

(setf  theta-2        (third  state-vector)) 

(setf  theta-2-dot    (fourth  state-vector)) 

(setf  1        0.5) 

(setf  r        3) 

(+  (*  r  (sin  theta-1) ) 

(*  1  (sin  theta-2) ) ) ) 

(defun  compute-z  (state-vector) 

(setf  theta-1        (first   state-vector)) 

(setf  theta-1-dot    (second  state-vector) ) 

(setf  theta-2        (third   state-vector)) 

(setf  theta-2-dot    (fourth  state-vector)) 

(setf  1       0.5) 

(setf  r       3) 

(+  (*  r  (cos  theta-1) ) 

(*  1  (cos  theta-2) ) ) ) 


(defun  draw- inverted-pendulum  (halt-time  K-theta-1  K-theta-1-dot 

K-theta-2  K-theta-2-dot ) 
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(setf  robot-body  (make- instance  'rigid-body)) 
(initialize   robot-body) 

(setf  robot-leg  (make-instance  'rigid-body)) 
(initialize   robot-leg) 


(setf  camera-1  (make-instance  'strobe-camera)) 

(move  camera-1  (deg-to-rad  0) (deg-to-rad  0) (deg-to-rad  0)  -10  0  0) 

(setf  graph-1-window  (make-window-graph- 1) ) 
(draw- coordinate- axes  graph- 1 -window) 

(setf  graph-2 -window  (make-window-graph-2 ) ) 
(draw- coordinate-axes  graph- 2 -window) 

(do  ( (x  '(0.5  0  1.0  0)  (heun-step-constant-leg  x  K-theta-1  K-theta-1-dot 

K-theta-2  K-theta-2-dot) ) 
(old-x  ' (0.5  0  1.0  0)  x) 
(time  0  (+  time  1) ) ) 
( (>  time  halt-time)  'done) 

(move   robot-body  (deg-to-rad  0) (deg-to-rad  0)  (third  x) 

0  (compute-y  x)  (*  -l(compute-z  x) ) ) 

(setf  ( transf ormed-node-list  robot-leg)  (list  (list  0  0  0  1) 

(list  0  (*  r  (sin  (first  x) ) ) 
(*  -1   r  (cos  (first  x) ) )  1) 
(list  0  0  0  1) ) ) 

(setf  (polygon-list  robot-leg)  (list  (list  2  1))) 

(cw:clear  (camera-window  camera-1) ) 
(take-picture  camera-1  robot-body) 
(take-picture  camera-1  robot-leg) 

(draw-line-in-window  graph- 1 -window  80  (list  (*  time-step  (-  time  1)) 

(first  old-x) ) 
(list  (*  time-step  time) 
(first  x) ) ) 
(draw-line-in-window  graph-2 -window  80  (list  (*  time-step  (-  time  1)) 

(third  old-x) ) 
(list  (*  time-step  time) 
(third  x) ) ) ) ) 


(defun  make-window-graph- 1  () 


( cw : make-window-stream 


borders  5 

left  0 

bottom  5  0 

width  450 

height  450 

title  "Leg  Angle    (theta-1)  LAGRANGIAN" 

activate-p  t) )   ;Make  window  visible. 


(defun  make-window-graph-2  () 

(cw: make-window-stream  : borders  5 

:left  0 
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bottom  550 

width  450 

height  450 

title  "Body  Attitude    (theta-2)  LAGRANGIAN' 

activate-p  t)  )   ,-Make  window  visible. 
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******************************************************* 


File 
Author 


load-euler- files . cl 

Mehmet  Bediz 

Naval  Postgraduate  School 

Monterey,  CA  93943 

Summary      :  This  file  contains  functions  to  load  files. 
•••••••••••••••••••••••••A******************************************* 


Load  files 


(load  "harmonic-equation. cl " ) 
(load  "robot-kinematics . cl " ) 
(load  "euler-angle-rigid-body . cl " ) 


101 


********************************************************************** 

File         :  s trobe- camera. cl 
Author       :  Dr.  R.  McGhee 

Naval  Postgraduate  School 

Monterey,  CA  93943 
Summary      :  This  file  contains  strobe-camera  class  definitions. 

to********************************-************************************ 

(require  :xcw) 

(cw: initialize-common-windows) 

(defclass  strobe-camera  (rigid-body) 
( ( focal-length 

: accessor  focal-length 
: initf orm  6 ) 
( camera-window 
: accessor  camera-window 

: initf orm  ( cw : make-window- s tream  rborders  5 

:left  470 
rbottom  500 
:width  650 
rheight  650 
: title  "Two  Link  Inverted  Pendulum 

(Constant  Length  Massless  Leg) 
LAGRANGIAN" 
: activate-p  t) ) 
(H-matrix 

:initform  (homogeneous -trans form  .3  -.3  0  -300  -90  -90)) 
( inverse-H-matrix 
: accessor  inverse-H-matrix 

:initform  (inverse-H  (homogeneous- trans form  .3  -.3  0  -300  -90  -90))) 
(enlargement- factor 
: accessor  enlargement- factor 
:initform  30) ) ) 

(defmethod  move  ((camera  strobe- camera)  azimuth  elevation  roll  x  y  z) 

(setf  (H-matrix  camera)  (homogeneous-transform  azimuth  elevation  roll  x  y  z) ) 
(setf  (inverse-H-matrix  camera)  (inverse-H  (H-matrix  camera)))) 

(defmethod  take-picture  ( (camera  strobe-camera)  (body  rigid-body) ) 
(let  ((camera-space-node-list  (mapcar  #' (lambda  (node-location) 
(post-multiply  (inverse-H-matrix  camera)  node-location)) 
( transformed-node-list  body)))) 
(dolist  (polygon  (polygon-list  body) ) 

(clip-and-draw-polygon  camera  polygon  camera-space-node-list) ) ) ) 

(defmethod  clip-and-draw-polygon 

( (camera  strobe-camera)  polygon  node-coord-list) 
(do*  ((initial-point  (nth  (first  polygon)  node-coord-list)) 
(from-point  initial-point  to-point) 

(remaining-nodes  (rest  polygon)  (rest  remaining-nodes) ) 
(to-point  (nth  (first  remaining-nodes)  node-coord-list) 
(if  (not  (null  (first  remaining-nodes))) 

(nth  (first  remaining-nodes)  node-coord-list)))) 
(-(null  to-point) 
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(draw-clipped-projection  camera  from-point  initial-point) ) 
(draw-clipped-projection  camera  from-point  to-point) ) ) 

(defmethod  draw-clipped-projection  ( (camera  strobe-camera)  from-point  to-point) 
(cond  ((and  (<=  (first  from-point)  (focal-length  camera)) 

(<=  (first  to-point)  (focal-length  camera)))  nil) 
( (<=  (first  from-point)  (focal-length  camera)) 
(draw-line-in-camera-window  camera 

(perspective-transform  camera  (from-clip  camera  from-point  to-point)) 
(perspective-transform  camera  to-point) ) ) 
( (<=  (first  to-point)  (focal-length  camera)) 
(draw-line-in-camera-window  camera 

(perspective-transform  camera  from-point) 

(perspective-transform  camera  (to-clip  camera  from-point  to-point)))) 
(t  (draw-line-in-camera-window  camera 

(perspective- transform  camera  from-point) 
(perspective- transform  camera  to-point) ) ) ) ) 

(defmethod  from-clip  ( (camera  strobe-camera)  from-point  to-point) 
(let  ((scale-factor  (/  (-  (focal-length  camera)  (first  from-point)) 

(-  (first  to-point)  (first  from-point))))) 
(list  (+  (first  from-point) 

(*  scale-factor  (-  (first  to-point)  (first  from-point)))) 
(+  (second  from-point) 

(*  scale-factor  (-  (second  to-point)  (second  from-point)))) 
(+  (third  from-point) 

(*  scale-factor  (-  (third  to-point)  (third  from-point))))  1))) 

(defmethod  to-clip  ( (camera  strobe-camera)  from-point  to-point) 
(from-clip  camera  to-point  from-point)) 


(defmethod  draw-line-in-camera-window  ( (camera  strobe- camera)  start  end) 
(cw: draw- line  (camera-window  camera) 

:x  (+  150  (first  start)) 
:y  (+  150  (second  start) ) ) 
:x  (+  150  (first  end) ) 
:y  (+  150  (second  end) ) ) 
: brush-width  0) ) 


( cw : make-pos  i  tion 
(cw: make-posit ion 


(defmethod  perspective-transform  ( (camera  strobe-camera)  point-in-camera-space) 
(let*  ( (enlargement- factor  (enlargement-factor  camera)) 
(focal-length  (focal-length  camera)) 

(x  (first  point-in-camera-space))   ;x  axis  is  along  optical  axis 
(y  (second  point-in-camera-space) )  ;y  is  out  right  side  of  camera 
(z  (third  point-in-camera-space)))  ;  z  is  out  bottom  of  camera 
(list  (+  (round  (*  enlargement- factor  (/  (*  focal-length  y)  x) ) ) 

150)   ; to  right  in  camera  window 
(+  150  (round  (*  enlargement- factor  (/  (*  focal-length  (-  z) )  x) ) 
) ) ) ) )   ; up  in  camera  window 

(defun  test-camera  (z  theta)  ; Produces  top  view  of  default  rigid-body, 
(setf  robot-leg  (make-instance  'rigid-body)) 
(initialize   robot-leg) 

(set-transf ormed-node-list-z  robot-leg  z) 
(set-transf ormed-node-list-theta  robot-leg  theta) 


103 


(setf  camera-1  (make-instance  'strobe-camera)) 

(move  camera-1  (deg-to-rad  0) (deg-to-rad  0) (deg-to-rad  0)  -30  0  0) 

(take-picture  camera-1  robot-leg)) 

(defun  deg-to-rad  (angle)  (*  .01745329251994329  angle)) 

(defun  animation  () 

(do  ((theta-loop  0  (+  theta-loop  1))) 
((>  theta-loop  90)  'end) 
(test-camera  30  theta-loop) ) ) 
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TWO  LINK  INVERTED  PENDULUM  SIMULATION  WITH  NEWTON- EULER 


Author 


Modified  by 
Summary 


********************************************************************* 
File         :  euler-angle-rigid-body . cl 

Dr .  R .  McGhee 

Naval  Postgraduate  School 

Monterey,  CA  93943 

Mehmet  Bediz 

This  file  contains  rigid  body  class  which  is  implemented 

by  Dr.  R.  McGhee.  The  function  "draw-inverted-pendulum" 

and  the  other  functions  called  by  this 

function  are  implemented  by  Mehmet  Bediz. 
********************************************************************* 

(defclass  rigid-body 

() 
((posture  ;The  vector  (xe  ye  ze  phi  theta  psi). 

:initform  '(000000) 

:initarg  :posture 

:accessor  posture) 
(posture-rate  ; The  vector  (xe-dot  ye-dot  ze-dot  phi-dot  theta-dot  psi-dot) . 

:initarg  :posture-rate 

:accessor  posture-rate) 
(velocity  ;The  six-vector  (u  v  w  p  q  r)  in  body  coordinates. 

: initform  '(000   000) 

: initarg  : velocity 

: accessor  velocity) 
(velocity-growth-rate   ;The  vector  (u-dot  v-dot  w-dot  p-dot  q-dot  r-dot) . 

: accessor  velocity-growth-rate) 
( forces-and- torques     ;The  vector  (Fx  Fy  Fz  L  M  N)  in  body  coordinates. 

: initform  (list  0  0  0  0  0  0) 

: accessor  forces-and- torques) 
(moments-of-inertia     ;The  vector  (Ix  Iy  Iz)  in  principal  axis  coordinates. 

: initform  ' (1  100  1) 

: initarg  :moments-of-inertia 

: accessor  moments-of-inertia) 
(mass 

: initform  100 

: initarg  :mass 

: accessor  mass) 
(node-list     ; (x  y  z  1)  in  body  coord  for  each  node.  Starts  with  (0  0  0  1) . 

:initform  '((0  0  0  1)  (-0.5  -0.5  0.5  1)  (0.5  -0.5  0.5  1)(0.5  -0.5  -0.5  1) 

(-0.5  -0.5  -0.5  1M-0.5   0.5  0.5  1)  (0.5   0.5  0.5  1) 
(0.5   0.5  -0.5  1)  (-0.5   0.5  -0.5  1)(0  0  0  1)(0  0  3  1)  ) 

:  initarg    mode-list 

:accessor  node-list) 
(polygon-list 

rinitform  '((1  5  8  4) (5  6  7  8)(6  2  3  7)(2  1  4  3)(9  10)) 

: initarg  :polygon-list 

:accessor  polygon-list) 
(transformed-node-list   ; (x  y  z  1)  in  earth  coord  for  each  node  in  node-list. 

:initform  '((0  0  0  1)  (-2  -2  0  1)  (2  -2  0  1)(2  -2  -30  l)(-2  -2  -30  1) 

(-2   2  0  1)  (2   2  0  1) (2   2  -30  1) (-2   2  -30  1) ) 

: accessor  transformed-node-list) 
(H-matrix 
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:initform  (unit-matrix  4) 
: accessor  H-matrix) 
(time-stamp 
raccessor  time-stamp))) 

(defmethod  initialize  ( (body  rigid-body) ) 

(setf  ( transf ormed-node-list  body)  (node-list  body)) 

(setf  (velocity-growth-rate  body)  (update-velocity-growth-rate  body) ) 

(setf  (posture-rate  body)  (earth-velocity  body) ) 

(setf  (time-stamp  body)  (get-internal-real-time)))) 

(defmethod  set-transf ormed-node-list-z ( (body  rigid-body)  z) 
(setf  ( third ( fourth  (transf ormed-node-list  body)))(-  z) ) 
(setf  (third(fifth  (transf ormed-node-list  body)))  (-  z) ) 
(setf  (third (eighth  (transf ormed-node-list  body)))  (-  z) ) 
(setf  (third(ninth  (transf ormed-node-list  body)))  (-  z)) 
(transf ormed-node-list  body))) 

(defun  transform  (obj) 

(list  0  (first  obj) (second  obj) (third  obj))) 

(defun  reverse-transform  (obj) 

(list  (second  obj) (third  obj) (fourth  obj)  1)) 

(defmethod  move  ( (body  rigid-body)  azimuth  elevation  roll  x  y  z) 
(setf  (posture  body)  (list  x  y  z  roll  elevation  azimuth)) 
(setf  (H-matrix  body) 

(homogeneous-transform  azimuth  elevation  roll  x  y  z) ) 
(transform-node-list  body)) 

(defmethod  get-delta-t  ((body  rigid-body))  0.005) 

(defmethod  forces-earth-to-body  (Fx-e  Fy-e  Fz-e  theta) 
(let*  ((forces-earth  (list  Fx-e  Fy-e  Fz-e)) 

(R-matrix  (rotation-matrix  0  (-  theta)  0)) 
(forces-body  (post-multiply  R-matrix  forces-earth))) 

forces-body) ) 

(defmethod  update-rigid-body  ((body  rigid-body))      ;Euler  integration, 
(let*  ( (delta-t  (get-delta-t  body))) 

(update-posture  body  delta-t) ;  EULER 

(update-velocity  body  delta-t) ;  EULER 

(setf  (H-matrix  body)  (homogeneous- transf orm  (sixth  (posture  body) ) 

(fifth  (posture  body))  (fourth  (posture  body))  (first  (posture  body)) 
(second  (posture  body))  (third  (posture  body)))) 
(transform-node-list  body) 
(update-velocity-growth-rate  body) ) ) 

(defmethod  update-velocity-growth-rate  ( (body  rigid-body) ) 

(setf  (velocity-growth-rate  body)  ;Assumes  principal  axis  coordinates  with 
(multiple-value-bind  ; origin  at  center  of  gravity  of  body. 

(Fx  Fy  Fz  L  M  N    u  v  w  p  q  r    Ix  Iy  Iz)  /Declares  local  variables, 
(values-list  ,-Values  assigned. 
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(list    (+ 

* 

* 

(  + 

* 

* 

(  + 

* 

* 

(/ 

+ 

(/ 

+ 

(/ 

+ 

(append 

( forces-and-torques  body)  (velocity  body)  (moments-of -inertia  body))) 
v  r)  (*  -1  w  q)  (/  Fx  (mass  body)) 
♦gravity*  (first  (third  (H-matrix  body) ))) ) 
w  p)  (*  -1  u  r)  (/  Fy  (mass  body)) 
♦gravity*  (second  (third  (H-matrix  body) ) ) ) ) 
u  q)  (*  -1  v  p)  ( /  Fz  (mass  body)) 
♦gravity*  (third  (third  (H-matrix  body) ))) ) 
(*  (-  Iy  Iz)  q  r)  L)  Ix) 
(*  (-  Iz  Ix)  r  p)  M)  Iy) 
(*  (-  Ix  Iy)  p  q)  N)  Iz)))))   ,-Value  returned. 

(defmethod  update-velocity  (  (body  rigid-body)  delta-t)  ,-Euler  integration, 
(setf  (velocity  body) 

(vector-add  (velocity  body) 

(scalar-multiply  delta-t  (velocity-growth-rate  body) ) ) ) ) 

(defmethod  update-posture  ((body  rigid-body)  delta-t)  ,-Euler  integration, 
(setf  (posture-rate  body)  (earth-velocity  body) ) 
(setf  (posture  body) 

(vector-add  (posture  body)  (scalar-multiply  delta-t  (posture-rate  body))))) 

(defmethod  transform-node-list  ( (body  rigid-body) ) 
(setf  ( transformed-node-list  body) 
(raapcar  #' (lambda  (node-location) 

(post-multiply  (H-matrix  body)  node-location) ) 
(node-list  body) ) ) ) 

(defconstant  *gravity*  32.2185) 

(defmethod  earth-velocity  ( (body  rigid-body) ) 

(let*  ((linear-velocity  (firstn  3  (velocity  body))) 
(rotational-velocity  (cdddr  (velocity  body) ) ) 
(posture  (posture  body) ) 
(R-matrix  (rotation-matrix  (sixth  posture)  (fifth  posture) 

(fourth  posture))) 
(linear-earth-velocity  (post-multiply  R-matrix  linear-velocity)) 
(T-matrix  (body-rate-to-euler-rate-matrix  (sixth  posture) 

(fifth  posture)  (fourth  posture))) 
(rotational-earth-velocity  (post-multiply  T-matrix 

rotational-velocity) ) ) 
(append  linear-earth-velocity  rotational-earth-velocity) ) ) 

(defun  test-rigid-body  () 

(setf  airplane-1  (make-instance  'rigid-body)) 

(initialize  airplane-1) 

(setf  camera-1  (make-instance  ' strobe- camera ) ) 

(move  camera-1  (-  (/  pi  2))  0   0  0  30  0  ) 

(take-picture  camera-1  airplane-1) 

(dotimes  (i  20  'done)  (update-rigid-body  airplane-1)) 

(take-picture  camera-1  airplane-1)) 
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(setf 

m 

(setf 

1 

(setf 

r 

(setf 

I 

(setf 

M 

two  Link  Inverted  Pendulum 

Massless  leg  and  a  single  rigid  body  (fixed  leg  length) 
state  vector  x  =  (theta-1  theta-1-dot  theta-2  theta-2-dot) 
state  vector  u  =  (M-l  M-2) 

-  control  torque  at  the  ankle  (M-l) 

-  control  torque  at  the  hip  (M-2) 
Newton-Euler  Method 

3x3  matrix  inversion  to  calculate  forces  and  moments 
Heun  Integration 


(setf   1        0.5) 
(setf   r        3) 
(setf  time-step  0.01) 

(defun  solve-three-equation-system  (state-vector  K-theta-1  K-theta-1-dot 

K-theta-2  K-theta-2-dot) 
100) 
0.5) 
3) 

100) 
M-l  (compute-M-1  state-vector  K-theta-1  K-theta-1-dot 

K-theta-2  K-theta-2-dot) ) 
(setf   M-2  (compute-M-2  state-vector  K-theta-1  K-theta-1-dot 

K-theta-2  K-theta-2-dot) ) 

.(first   state-vector)) 

(second  state-vector) ) 

(third   state-vector)) 

(fourth  state-vector)) 


(post -multiply 

(matrix- inverse 

(list  (list   0      I      (*  -1   1  (sin  (-  theta-2  theta-1)))) 
(list  (*  m   r  (cos  theta-1))   (*  m   1  (cos  theta-2)) 

(*  -1  (sin  theta-1) ) ) 
(list  (*  m   r  (sin  theta-1))   (*  m   1  (sin  theta-2)) 
(cos  theta-1) ) ) ) 

(list   (/  (*  (-  (*  M-2  (+  1  r) )  (*  1  M-l) ) (cos  (-  theta-2  theta-1)))  r) 

(+  (*  m  r  theta-1-dot  theta-1-dot  (sin  theta-1)) 

(*  m  1  theta-2-dot  theta-2-dot  (sin  theta-2)) 

(/  (*  (-  M-l  M-2)  (cos  theta-1) )  r) ) 

(+  (*  -1  m  r  theta-1-dot  theta-1-dot  (cos  theta-1)) 

(*  -1  m  1  theta-2-dot  theta-2-dot  (cos  theta-2)) 

(*  m  g) 

(/  (*  (-  M-l  M-2)  (sin  theta-1) )  r) ) ) ) ) 


(defun  euler-step-constant-leg  (state-vector  threeXthree-matrix) 
(mapcar  '+  state-vector  (scalar-multiply  time-step 

(derivative- state- vector 
state-vector  threeXthree-matrix) ) ) ) 
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(setf 

theta-1 

(setf 

theta-1- 

-dot 

(setf 

theta-2 

(setf 

theta-2- 

-dot 

(setf 

g     32 

.2) 

(defun  heun-step-constant-leg  (x  K-theta-1  K-theta-1-dot 

K-theta-2  K-theta-2-dot) 
(setf  threeXthree-matrix  (solve-three-equation-system 

x  K-theta-1  K-theta-1-dot 
K-theta-2  K-theta-2-dot) ) 
(mapcar  '+  x  (scalar-multiply  (*  time-step  .5) 

(vector-add  (derivative-state-vector  x  threeXthree-matrix) 
(derivative- state- vector 

(euler-step-constant-leg  x  threeXthree-matrix) 
threeXthree-matrix) ) ) ) ) 

(defun  derivative-state-vector  (state-vector  threeXthree-matrix) 
(list  (second  state-vector) 

(first  threeXthree-matrix) 
(fourth  state-vector) 
(second  threeXthree-matrix) ) ) 


(defun  compute-M-1  (state-vector  K-theta-1  K-theta-1-dot 

K-theta-2  K-theta-2-dot) 
(setf  theta-1  (first  state-vector)) 
(setf  theta-1-dot  (second  state-vector)) 
(+  (*  -1  K-theta-1  theta-1) (*  -1  K-theta-1-dot  theta-1-dot ) ) ) 

(defun  compute-M-2  (state-vector  K-theta-1  K-theta-1-dot 

K-theta-2  K-theta-2-dot) 
(setf  theta-2  (third  state-vector)) 
(setf  theta-2-dot  (fourth  state-vector)) 
(+  (*  -1  K-theta-2  theta-2) (*  -1  K-theta-2-dot  theta-2-dot) ) ) 

(defun  compute-y  (state-vector) 

(setf  theta-1        (first   state-vector)) 

(setf  theta-1-dot    (second  state-vector) ) 

(setf  theta-2        (third   state-vector) ) 

(setf  theta-2-dot    (fourth  state-vector)) 

(setf  1        0.5) 

(setf  r        3) 

(+  (*  r  (sin  theta-1) ) 

(*  1  (sin  theta-2) ) ) ) 

(defun  compute-z  (state-vector) 

(setf  theta-1        (first   state-vector)) 

(setf  theta-1-dot    (second  state-vector) ) 

(setf  theta-2        (third   state-vector)) 

(setf  theta-2-dot    (fourth  state-vector)) 

(setf  1        0.5) 

(setf  r        3) 

(+  (*  r  (cos  theta-1) ) 

(*  1  (cos  theta-2) ) ) ) 


(defun  draw- inverted-pendulum  (halt-time  K-theta-1  K-theta-1-dot 

K-theta-2  K-theta-2-dot) 
(setf  robot-body  (make-instance  'rigid-body)) 
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(initialize   robot-body) 

(setf  robot-leg  (make- instance  'rigid-body)) 
(initialize   robot-leg) 


(setf  camera-1  (make- instance  'strobe-camera)) 

(move  camera-1  (deg-to-rad  0) (deg-to-rad  0) (deg-to-rad  0)  -10  0  0) 

(setf  graph- 1 -window  (make-window-graph- 1 ) ) 
(draw- coordinate-axes  graph- 1 -window) 

(setf  graph- 2 -window  (make-window-graph-2 ) ) 
(draw- coordinate-axes  graph- 2 -window) 

(do  ( (x  '(0.5  0  1.0  0)  (heun-step-constant-leg  x  K-theta-1  K-theta-1-dot 

K-theta-2  K-theta-2-dot) ) 
(old-x  ' (0.5  0  1.0  0)  x) 
(time  0  (  +  time  1) ) ) 
( (>  time  halt-time)  'done) 

(move   robot-body  (deg-to-rad  0 ) (deg-to-rad  0)  (third  x) 

0  (compute-y  x)  (*  -l(compute-z  x) ) ) 

(setf  (transformed-node-list  robot-leg)  (list  (list  0  0  0  1) 

(list  0  (*  r  (sin  (first  x) ) ) 
(*  -1   r  (cos  (first  x) ) )  1) 
(list  0  0  0  1) ) ) 

(setf  (polygon-list  robot-leg)  (list  (list  2  1))) 

(cwiclear  (camera-window  camera-1)) 

(take-picture  camera-1  robot-body) 

(take-picture  camera-1  robot-leg) 

(draw-line-in-window  graph-1-window  80  (list  (*  time-step  (-  time  1)) 

(first  old-x) ) 
(list  (*  time-step  time) 

(first  x) ) ) 
(draw-line-in-window  graph-2-window  80  (list  (*  time-step  (-  time  1)) 

(third  old-x) ) 
(list  (*  time-step  time) 

(third  x) ) ) ) ) 

(defun  make-window-graph- 1  () 


(cw: make-window- stream 


borders  5 

left  0 

bottom  50 

width  450 

height  450 

title  "Leg  Angle    (theta-1)  NEWTON-EULER' 

activate-p  t)  )   ,-Make  window  visible. 


(defun  make-window-graph-2  () 

( cw : make-window- s tream  :borders  5 

:left  0 
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bottom  550 

width  450 

height  450 

title  "Body  Attitude    (theta-2)  NEWTON-EULER' 

activate-p  t))   ;Make  window  visible. 


Ill 


************************************** 


File 
Author 


load-euler- files . cl 

Mehmet  Bediz 

Naval  Postgraduate  School 

Monterey,  CA  93943 
Summary      :  This  file  contains  functions  to  load  files. 
••••••A************************************************************** 


Load  files 


(load  "harmonic -equation. cl " ) 

(load  "robot-kinematics . cl" ) 

(load  "euler-angle- rigid-body. cl " ) 

(load  "strobe-camera . cl " ) 
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♦  It****************************************** 


File 
Author 


Summary 


strobe-camera . cl 

Dr.  R.  McGhee 

Naval  Postgraduate  School 

Monterey,  CA  93943 

This  file  contains  strobe-camera  class  definitions. 


■****************************+*********************************»*** 


(require  :xcw) 

(cw: initialize -common -windows ] 


(defclass  strobe-camera  (rigid-body) 
(  (focal-length 

: accessor  focal-length 
: initf orm  6) 
(camera-window 
: accessor  camera-window 
rinitform  (cw:make-window-stream 


: borders  5 

:left  470 

: bottom  500 

:width  650 

:height  650 

: title  "Two  Link  Inverted  Pendulum 

(Constant  Length  Massless  Leg) 
NEWTON-EULER" 
: activate-p  t) ) 


(H-matrix 

:initform  (homogeneous-transform  .3 
(inverse-H-matrix 

: accessor  inverse-H-matrix 

:initform  (inverse-H  (homogeneous-transform 
(enlargement -factor 

: accessor  enlargement-factor 

rinitform  30) ) ) 


3  0  -300  -90  -90) ) 


3  -.3  0  -300  -90  -90) ) ) 


(defmethod  move  ((camera  strobe-camera)  azimuth  elevation  roll  x  y  z) 

(setf  (H-matrix  camera)  (homogeneous-transform  azimuth  elevation  roll  x  y  z) ) 
(setf  (inverse-H-matrix  camera)  (inverse-H  (H-matrix  camera)))) 

(defmethod  take-picture  ( (camera  strobe-camera)  (body  rigid-body) ) 
(let  ((camera-space-node-list  (mapcar  #' (lambda  (node-location) 
(post-multiply  (inverse-H-matrix  camera)  node-location)) 
(transformed-node-list  body) ) ) ) 
(dolist  (polygon  (polygon-list  body) ) 

(clip-and-draw-polygon  camera  polygon  camera-space-node-list) ) ) ) 

(defmethod  clip-and-draw-polygon 

((camera  strobe-camera)  polygon  node-coord-list) 
(do*  ((initial-point  (nth  (first  polygon)  node-coord-list)) 
(from-point  initial-point  to-point) 

(remaining-nodes  (rest  polygon)  (rest  remaming-nodes)  ) 
(to-point  (nth  (first  remaining-nodes)  node-coord-list) 
(if  (not  (null  (first  remaining-nodes))) 

(nth  (first  remaining-nodes)  node-coord-list)))) 
( (null  to-point) 

(draw-clipped-projection  camera  from-point  initial-point)) 
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(draw-clipped-projection  camera  from-point  to-point) ) ) 

(defmethod  draw-clipped-projection  ( (camera  strobe-camera)  from-point  to-point) 
(cond  ((and  (<=  (first  from-point)  (focal-length  camera)) 

(<=  (first  to-point)  (focal-length  camera)))  nil) 
( (<=  (first  from-point)  (focal-length  camera)) 
(draw-line-in-camera-window  camera 

(perspective- transform  camera  (from-clip  camera  from-point  to-point)) 
(perspective-transform  camera  to-point) ) ) 
( (<=  (first  to-point)  (focal-length  camera)) 
(draw-line-in-camera-window  camera 

(perspective- transform  camera  from-point) 

(perspective- transform  camera  (to-clip  camera  from-point  to-point) ) ) ) 
(t  (draw-line-in-camera-window  camera 

(perspective-transform  camera  from-point) 
(perspective-transform  camera  to-point) ) ) ) ) 

(defmethod  from-clip  ( (camera  strobe-camera)  from-point  to-point) 

(let  ((scale-factor  (/  (-  (focal-length  camera)  (first  from-point)) 

(-  (first  to-point)  (first  from-point))))) 
(list  (+  (first  from-point) 

(*  scale-factor  (-  (first  to-point)  (first  from-point)))) 
(+  (second  from-point) 

(*  scale-factor  (-  (second  to-point)  (second  from-point)))) 
(+  (third  from-point) 

(*  scale-factor  (-  (third  to-point)  (third  from-point))))  1))) 

(defmethod  to-clip  ( (camera  strobe-camera)  from-point  to-point) 
(from-clip  camera  to-point  from-point)) 


(defmethod  draw-line-in-camera-window  ( (camera  strobe-camera)  start  end) 
(cw: draw- line  (camera-window  camera) 

:x  (+  150  (first  start) ) 
:y  (+  150  (second  start))) 
:x  (+  150  (first  end) ) 
:y  (+  150  (second  end))) 


(cw: make-posit ion 
(cw: make-position 
: brush-width  0) ) 


(defmethod  perspective-transform  ( (camera  strobe-camera)  point-in-camera-space) 
(let*  ((enlargement-factor  (enlargement- factor  camera)) 
(focal-length  (focal-length  camera)) 

(x  (first  point-in-camera-space))   ;x  axis  is  along  optical  axis 
(y  (second  point-in-camera-space) )  ;y  is  out  right  side  of  camera 
(z  (third  point-in-camera-space)))  ;  z  is  out  bottom  of  camera 
(list  (+  (round  (*  enlargement-factor  (/  (*  focal-length  y)  x) ) ) 

150)  ; to  right  in  camera  window 

(+  150  (round  (*  enlargement-factor  (/  (*  focal-length  (-  z) )  x) ) 
)  )  )  )  )   ,-up  in  camera  window 

(defun  test-camera  (z  theta)  ; Produces  top  view  of  default  rigid-body, 
(setf  robot-leg  (make- instance  'rigid-body)) 
(initialize   robot-leg) 

(set-transf ormed-node-list-z  robot-leg  z) 
(set-transf ormed-node-list-theta  robot-leg  theta) 
(setf  camera-1  (make- instance  'strobe-camera)) 
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(move  camera-1  (deg-to-rad  0) (deg-to-rad  0) (deg-to-rad  0)  -30  0  0) 
(take-picture  camera-1  robot-leg)) 

(defun  deg-to-rad  (angle)  (*  .01745329251994329  angle)) 

(defun  animation  () 

(do  ((theta-loop  0  (+  theta-loop  1))) 
( (>  theta-loop  90)  'end) 
(test-camera  30  theta-loop))) 
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APPENDIX  B:  KINEMATIC  SIMULATION  SOFTWARE 


//*******************+******  ************  it****************:************** 

//  File  :  main.C 

//  Author  :  Mehmet  Bediz 

//  :  Naval  Postgraduate  School 

//  :  Monterey,  CA  93943 

//Created  :  November  1996 

//  Summary  :  This  file  contains  the  main  function  and  three  motion 

//  :  functions  of  the  kinematic  model  Dynaman:  forward  stepping, 

//  :  upward  stepping,  and  jumping. 

#include  <string.h> 

#include  "main.h" 

void  step_forward(int  number_of_steps,  float  X,  float  Y,  float  Z); 

void  step_upward(int  number_of_steps,  float  X,  float  Y,  float  Z); 

void  jump(float  X.float  Y,float  Z); 

//  Speed  of  the  animation  can  be  controlled  by  changing  delta_t 
float  delta.t  =  2  *  3.0; 


pfNode 

*root; 

pfDCS 

*dcs; 

pfMatrix 

mat,  orbit; 

pf  Sphere 

sphere; 

pfNode 

*modell,  *model2,  *model3.  *model4; 

pfNode 

*model8,  *modell0; 

pfNode 

*modell  1,  *modell2,  *modell3,  *modell4 

pfDCS 

*nodel 1,  *nodel2,  *nodel3,  *nodel4; 

pfDCS 

*nodel,  *node2,  *node3,  *node4; 

pfDCS 

*node£ 

,  *nodel0; 

pfDCS 

*dcs0; 

pfDCS 

*dcsl,  *dcs2,  *dcs3; 

pfDCS 

*dcs4,  *dcs5,  *dcs6; 

pfDCS 

*dcs7. 

*dcs8,  *dcs9,  *dcsl0; 

pfDCS 

*dcsl 1 

,  *dcsl2,  *dcsl3; 

pfDCS 

*dcsl4 

,  *dcsl5,  *dcsl6; 

pfDCS 

*dcs30 

,  *dcs31,  *dcs32,  *dcs33; 

pfDCS 

*dcs34 

,  *dcs35,  *dcs36; 

pfDCS 

*dcs37 

,  *dcs38,  *dcs39; 

char 

*filel,*file2,  *file3,  *file4; 

char 

*file8,  *filel0; 

char 

*filell,  *filel2,  *filel3,  *filel4; 

pfLightSource      * 

light; 

pfMatrix 

lightMat; 

float 

d; 

pfTexture 

*tex; 

pfFrustum 

*Frust; 

void 

*  arena; 

pfDCS 

*lightDCS; 
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int  i; 

float  final_position; 

// — 

//    Function:  main 

//    Returns:  None 

//   Parameters:        None 

//    Summary:  Initializes  IRIS  Performer™,  loads  body  parts  from  poly  format 

//  files  to  DCS  nodes,  creates  the  channels  at  the  upper  right 

//  corner  of  the  window,  calls  step_forward,  step_upward,  and 

//  jump  functions  with  the  number  of  steps  and  the  initial 

//  positions. 

// 

void 

main  (int  argc,  char  *argv[]) 


/*  choose  default  objects  of  none  specified  */ 

filel  =  "lowerleg.poly"; 

file2  =  "upperleg.poly"; 

file3  =  "head.iv"; 

file4=  "torso.poly"; 

file8  =  "foot.poly"; 

file  10  =  "floor.iv"; 

filel  1  =  "rightupperarm.poly"; 

filel  2  =  "lowerarm.poly"; 

filel  3  =  "hand.poly"; 

filel4  =  "leftupperarm.poly"; 

if  (  !  strcmp(argv[l],"slow")){ 
delta_t  =  2  *  0.5; 


#ifndef  IRISGL 

printf("Sorry,  shadows  doesn't  work  in  OPENGLXn"); 

return  0; 
#endif 

/*  Initialize  Performer  */ 
pflnitO; 

/*  Use  default  multiprocessing  mode  based  on  number  of 

*  processors. 
*/ 

pfMultiprocess(PFMP_DEFAULT); 

/**  allocate  shared  memory    **/ 
InitSharedQ; 

/*  Configure  multiprocessing  mode  and  start  parallel 

*  processes. 
*/ 

pfConfigQ; 
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/**  Initialize  Performer  utility  and  GUI  functions  **/ 
pfuInitUtilO; 

pflnitClock(O.Of); 
Shared->simTime  =  pfGetTime(); 
srand(Shared->simTime*  1 0000000); 

/*  Append  to  PFPATH  additional  standard  directories  where 

*  geometry  and  textures  exist 
*/ 

pfFilePath(".:/usr/share/Performer/data:/tmp_mnt/workb/bediz/Performer/data"); 

/*  Do  not  use  FLAT_  primitives  because  they  look  bad 

*  with  local  lighting. 
*/ 
pfdBldrMode(PFDBLDR_MESH_LOCAL_LIGHTING,  1 ); 

/*  Read  a  single  file,  of  any  known  type.  */ 
if  ((root  =  pfdLoadFile("text.iv"))  ==  NULL) 

{ 

pfExit(); 

exit(-l); 
} 

/*  Load  the  files  */ 

if  ((model  1  =  pfdLoadFile(filel))  ==  NULL) 

{ 

pfExit(); 
exit(-l); 

} 

if  ((model2  =  pfdLoadFile(file2))  ==  NULL) 

{ 

pfExit(); 
exit(-l); 

} 

if  ((model3  =  pfdLoadFile(file3))  =  NULL) 

{ 

pfExitO; 
exit(-l); 

} 

if  ((model4  =  pfdLoadFile(file4))  =  NULL) 

{ 

pfExitO; 
exit(-l); 

} 

if  ((model8  =  pfdLoadFile(file8))  =  NULL) 

{ 

pfExitO; 
exit(-l); 

} 

if  ((model  10  =  pfdLoadFile(filelO))  ==  NULL) 

{ 

pfExitO; 

exit(-l); 
} 
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if  ((model  1 1  =  pfdLoadFile(filel  1))  =  NULL) 

{ 

pfExitO; 

exit(-l); 

} 

if  ((model  12  =  pfdLoadFile(filel2))  ==  NULL) 

{ 

pfExit(); 
exit(-l); 

} 

if  ((model  13  =  pfdLoadFile(filel3))  ==  NULL) 

{ 

pfExit(); 
exit(-l); 

} 

if  ((modell4  =  pfdLoadFile(filel4))  ==  NULL) 

{ 

pfExitO; 

exit(-l); 
} 

/*  Create  and  attach  loaded  subgraph  to  a  pfScene.  */ 
Shared->scene  =  new  pfScene; 
dcs  =  new  pfDCS; 
dcs->addChild(root); 

Shared->scene->addChild(dcs); 

/*  determine  extent  of  scene's  geometry  */ 

Shared->scene->getBound(&(Shared->bsphere)); 

/*******************  MAIN  WINDOW  ****************/ 

/*  Configure  and  open  GL  window  */ 

Shared->p  =  pfGetPipe(O); 

Shared->p2  =  pfGetPipe(O); 

Shared->pw  =  new  pfPipeWindow(Shared->p); 

Shared- >pw->setName("DYNAMAN"); 

Shared->pw->setConfigFunc(OpenPipeWin); 

Shared->pw->setOriginSize(120,  120,  1100,  1100); 

Shared->pw->config(); 

/*    initializes  mouse  and  keyboard  inputs  to  be  read  from  window  */ 
pfuInitInput(Shared->pw,  PFUINPUT_GL); 
pfuInitGUl(Shared->pw); 
pfuEnableGUI(TRUE); 

/*  Create  and  configure  a  pfChannel.  */ 
Shared->chan  =  new  pfChannel(Shared->p); 
Shared->chan->setScene(Shared->scene); 
Shared->chan->setNearFar(1.0f,  5.0f  *  Shared->bsphere.radius); 
Shared->chan->setFOV(45.0f,  O.Of); 
Shared->view.xyz.set(1.3f  *  Shared->bsphere.radius, 

-2.5f  *  Shared->bsphere. radius, 

3.0f  *  Shared->bsphere. radius); 
Shared->view.hpr.set(0.0f,  -45. Of,  0.0f); 
Shared->chan->setView(Shared->view.xyz,  Shared->view.hpr); 
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Shared->chan->setTravFunc(PFTRAV_DRAW,DrawChannel); 

/********  first  CHANNEL  **************************/ 
Shared->chan2[0]  =  new  pfChannel(Shared->p); 
Shared->pw->addChan(Shared->chan2[0]); 

Shared->chan2[0]->setTravFunc(PFTRAV_DRAW,DrawChannel); 
Shared->chan2[0]->setScene(Shared->scene); 
Shared->chari2[0]->setNearFar(1.0f,  5.0f  *  Shared->bsphere.radius); 
Shared->chan2[0]->setFOV(45.0f,  0.00; 
Shared->view.xyz.set(  Shared->bsphere. radius, 

-4. Of  *  Shared->bsphere. radius, 

0.8f  *  Shared->bsphere. radius); 
Shared->view.hpr.set(0.0f,  O.Of,  O.Of); 
Shared->chan2[0]->setView(Shared->view.xyz,  Shared->view.hpr); 

/********    SECOND  CHANNEL    **************************/ 

Shared->chan2[l]  =  new  pfChannel(Shared->p); 

Shared->pw->addChan(Shared->chan2[l]); 

Shared->chan2[l]->setTravFunc(PFTRAV_DRAW,DrawChannel); 

Shared->chan2[l]->setScene(Shared->scene); 

Shared->chan2[l]->setNearFar(1.0f,  5.0f  *  Shared->bsphere.radius); 

Shared->chan2[l]->setFOV(45.0f,0.0f); 

Shared-> view. xyz.set(  1.3  *  Shared->bsphere.radius, 

-2.5f  *  Shared->bsphere.radius, 

3.0f  *  Shared->bsphere.radius); 
Shared->view.hpr.set(0.0f,  -45.0f,  O.Of); 
Shared->chan2[  1  ]->setView(Shared->view.xyz,  Shared->view.hpr); 

/********    THIRD  CHANNEL      **************************/ 

Shared->chan2[2]  =  new  pfChannel(Shared->p); 
Shared->pw->addChan(Shared->chan2[2]); 

Shared->chan2[2]->setTravFunc(PFTRAV_DRAW,DrawChannel); 
Shared->chan2[2]->setScene(Shared->scene); 
Shared->chan2[2]->setNearFar(1.0f,  5. Of  *  Shared->bsphere.radius); 

Shared->chan2[2]->setFOV(45.0f,0.0f); 
Shared->view.xyz.set(   1.6    *  Shared->bsphere.radius, 

-0.1 5f  *  Shared->bsphere.radius, 

2.5f  *  Shared->bsphere.radius); 
Shared->view.hpr.set(0.0f,  -90.0f,  O.Of); 
Shared->chan2[2]->setView(Shared->view.xyz,Shared->  view.hpr); 

/********  FOURTH  CHANNEL      **************************/ 

Shared->chan2[3]  =  new  pfChannel(Shared->p); 
Shared->pw->addChan(Shared->chan2[3]); 

Shared->chan2[3]->setTravFunc(PFTRAV_DRAW,DrawChannel); 
Shared->chan2[3]->setScene(Shared->scene); 
Shared->chan2[3]->setNearFar(1.0f,  5.0f  *  Shared->bsphere.radius); 
Shared->chan2[3]->setFOV(45.0f,0.0f); 
Shared->view.xyz.set(  4.0f  *  Shared->bsphere. radius, 
-0.5f  *  Shared->bsphere. radius, 
0.5  *  Shared->bsphere.radius); 
Shared->view.hpr.set(90.0f,  O.Of,  O.Of); 
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Shared->chan2[3]->setView(Shared->view.xyz,  Shared->view.hpr); 
Shared->chan2[3]->setViewport(0.7,  0.85,  0.7,  0.85); 
Shared->chan2[2]->setViewport(0.85,  1.0,  0.7,  0.85); 
Shared->chan2[l]->setViewport(0.85,  1.0,0.85,  1.0); 
Shared->chan2[0]->setViewport(0.7,  0.85,  0.85,  1.0); 

orbit.makeRot(1.0f,  O.Of,  O.Of,  1.00; 

/*  scale  models  to  unit  size  */ 
nodel  =  new  pfDCS; 
node  1  ->addChild(model  1 ); 
model  l->getBound(&sphere); 
if  (sphere.radius  >  O.Of) 

nodel->setScale(1.0f/sphere.radius); 

node2  =  new  pfDCS; 
node2->addChild(model2); 
model2->getBound(&sphere); 
if  (sphere.radius  >  O.Of) 

node2->setScale(1.0f/sphere.radius); 

node3  =  new  pfDCS; 
node3->addChild(model3); 
model2->getBound(&sphere); 
if  (sphere.radius  >  O.Of) 

node3->setScale(  1  .Of/sphere.radius); 

node4  =  new  pfDCS; 
node4->addChild(model4); 
model4->getBound(&sphere); 
if  (sphere.radius  >  O.Of) 

node4->setScale(  1  .Of/sphere.radius); 

node8  =  new  pfDCS; 
node8->addChild(model8); 
model8->getBound(&sphere); 
if  (sphere.radius  >  O.Of) 

node8->setScale(  1  .Of/sphere.radius); 

node  10  =  new  pfDCS; 
node  1 0->addChild(model  10); 
model  1 0->getBound(&sphere); 
if  (sphere.radius  >  O.Of) 

node  10->setScale(l. Of/sphere. radius); 

nodel  1  =  new  pfDCS; 
nodel  l->addChild(modell  1); 
model  1  l->getBound(&sphere); 
if  (sphere.radius  >  O.Of) 

node  1 0->setScale(  1  .Of/sphere.radius); 

node  12  =  new  pfDCS; 
node  1 2->addChild(model  12); 
model  1 2->getBound(&sphere); 
if  (sphere.radius  >  0.00 
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node  10->setScale(l.  Of/sphere,  radius); 

node  13  =  new  pfDCS; 
node  1 3->addChild(model  13); 
model  1 3->getBound(&sphere); 
if  (sphere.radius  >  0.00 

node  1 0->setScale(  1  .Of/sphere,  radius); 

node  14  =  new  pfDCS; 
nodel4->addChild(modell4); 
model  1 4->getBound(&sphere) ; 
if  (sphere.radius  >  O.Of) 

nodelO->setScale(  1  .Of/sphere. radius); 

/*  Lighting  without  shadowing     */ 
//  put  a  default  light  source  in  the  scene 
Shared->scene->addChild(new  pfLightSource); 

arena  =  pfGetSharedArena(); 

//  Create  and  configure  shadow  frustum.  Fit  frustum  tightly  to  scene. 

Frust  =  new(arena)  pfFrustum; 

Frust->makeSimple(FOV); 

d  =  Shared->bsphere.radius  /  sinf(PF_DEG2RAD(FOV/2.0f)); 
d  =  PF_MAX2(d,  NEAR  +  Shared->bsphere.radius); 

Frust->setNearFar(NEAR,  d  +  l.lf  *  Shared->bsphere.radius); 
tex  =  initSpotTexO; 

light  =  new  pfLightSource; 
light->setMode(PFLS_PROJTEX_ENABLE,  1 ); 
light->setAttr(PFLS_PROJ_FRUST,  Frust); 
light->setAttr(PFLS_PROJ_TEX,  tex); 
light->setColor(PFLT_DIFFUSE,  l.Of,  l.Of,  1.00; 
light->setVal(PFLS_INTENSITY,  3.0f); 
light->setPos(17.0  ,  -7.0  ,  13.9,  l.Of);//  Make  light  local 
light->setSpotDir(O.Of,  O.Of,  -lO.Of); 

//  Make  DCS  to  move  lights  around 
lightDCS  =  new  pfDCS; 
lightDCS->addChild(light); 

/*  Main  DCS     */ 
dcsO  =  new  pfDCS; 

/*  Left  Foot     */ 
dcsl  =  new  pfDCS; 
dcsl  ->addChild(nodel); 

/*  Left  lowerleg  */ 

/*  Right  lowerleg  */ 
dcs4  =  new  pfDCS; 
dcs4  ->  addChild(nodel ); 
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/*  Left  upperleg  */ 
dcs2  =  new  pfDCS; 
dcs2  ->  addChild(node2); 

/*  Right  upperleg  */ 
dcs5  =  new  pfDCS; 
dcs5  ->  addChild(node2); 


/*  Floor  */ 
dcslO  =  new  pfDCS; 
dcslO  ->  addChild(nodelO 
dcs30  =  new  pfDCS; 
dcs30  ->  addChild(nodelO 
dcs31  =  new  pfDCS; 
dcs31  ->  addChild(nodelO 
dcs32  =  new  pfDCS; 
dcs32  ->  addChild(nodelO 
dcs33  =  new  pfDCS; 
dcs33  ->  addChild(nodelO 
dcs34  =  new  pfDCS; 
dcs34  ->  addChild(nodelO 
dcs35  =  new  pfDCS; 
dcs35  ->  addChild(nodelO 
dcs36  =  new  pfDCS; 
dcs36  ->  addChild(nodelO 
dcs37  =  new  pfDCS; 
dcs37  ->  addChild(nodelO 
dcs38  =  new  pfDCS; 
dcs38  ->  addChild(nodelO 
dcs39  =  new  pfDCS; 
dcs39  ->  addChild(nodelO 


/*  Torso  (Two  parts)  */ 
dcs6  =  new  pfDCS; 
dcs6  ->  addChild(node4); 
dcsO  ->  addChild(dcs6); 

dcs3  =  new  pfDCS; 
dcs3  ->  addChild(node4); 
dcsO  ->  addChild(dcs3); 

/*    Left  Legs  */ 
dcs2->addChild(dcsl); 
dcsO  ->  addChild(dcs2); 

/*  Right  Legs  */ 
dcs5  ->  addChild(dcs4); 
dcsO  ->  addChild(dcs5); 

/*  Feet  */ 
dcs8  =  new  pfDCS; 
dcs8  ->  addChild(node8); 
dcs9  =  new  pfDCS; 
dcs9  ->  addChild(node8); 
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dcsl  ->  addChild(dcs8); 
dcs4  ->  addChild(dcs9); 

/*    Head     */ 
dcs7  =  new  pfDCS; 
dcs7  ->  addChiId(node3); 
dcs3  ->  addChild(dcs7); 

/*    Right  upper  arm    */ 
dcsll  =newpfDCS; 
dcsll  ->addChild(nodell); 
dcsO->addChild(dcsll); 

/*    Right  lower  arm    */ 
dcsl2  =  newpfDCS; 
dcsl 2  ->  addChild(nodel2); 
dcsll  ->  addChild(dcsl2); 

/*    Right  hand    */ 
dcsl3  =  newpfDCS; 
dcsl 3  ->  addChild(nodel3); 
dcsl2->addChild(dcsl3); 

/*    Left  upper  arm    */ 
dcsl4  =  newpfDCS; 
dcsl4  ->  addChild(nodel4); 
dcsO->addChild(dcsl4); 

/*    Left  lower  arm    */ 
dcsl 5  =  new  pfDCS; 
dcsl5  ->  addChild(nodel2); 
dcsl4->addChild(dcsl5); 

/*    Left  hand    */ 
dcsl6  =  new  pfDCS; 
dcsl6  ->  addChild(nodel3); 
dcsl5->addChild(dcsl6); 


//  Stairs 

dcs30->addChild(dcs31) 
dcs31  ->  addChild(dcs32) 
dcs32  ->  addChild(dcs33) 
dcs33  ->  addChild(dcs34) 
dcs34  ->  addChild(dcs35) 
dcs35  ->  addChild(dcs36) 
dcs36  ->  addChild(dcs37) 
dcs37  ->  addChild(dcs38) 
dcs38  ->  addChild(dcs39) 


Shared->scene->  addChild(dcsO); 
Shared->scene->  addChild(dcs30); 

/*  Create  "floor  letters"  */ 
dcs  ->  addChild(initFloor(&Shared->bsphere)); 
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/*  Floor        */ 
dcsl0->setScale(2.0f); 
dcslO->setRot(0.0,  -90.0  ,0.0); 
dcsl0->setTrans(-1.0  ,  -11.0  ,  0.05); 


//  Draw  steps 

dcs30->setScale(0.5f); 

dcs30->setRot(-90.0,  -90.0  ,0.0); 

dcs30->setTrans(13.0  ,0.0,0.05); 

dcs31->setTrans(0.0 

dcs32->setTrans(0.0 

dcs33->setTrans(0.0 

dcs34->setTrans(0.0 

dcs35->setTrans(0.0 

dcs36->setTrans(0.0 

dcs37->setTrans(0.0 

dcs38->setTrans(0.0 

dcs39->setTrans(0.0 


-2.0 

5.4) 

-2.0 

4.1) 

-1.9 

4.1) 

-1.9 

4.1) 

-1.9 

4.1) 

-1.8 

4.1) 

-1.8 

4.1) 

-1.8 

4.1) 

-1.7 

4.1) 

/*      Set  up  initial/default  view  */ 
MakeGUI(); 

int  temp  =  system("sfplay  runaway.wav  &"); 
if(-l  =temp){ 
printf("Can't  play  \n"); 

} 

else{ 
printf("Playing  %i  \n",temp); 

} 

for  (int  forever  =  0;  forever  <  50;  forever  ++){ 

step_forward(6,  -9.8  ,  -7.0  ,  3.9); 

step_upward(5,  13.3  ,  -7.0  ,  3.9); 

jump(3 1.7, -7.0,12.496556); 

step_forward(4,  39.137990  ,  -7.0  ,  4.670563); 

} 

//  Kill  music 

system("music_killer"); 

/*  Terminate  parallel  processes  and  exit.  */ 

pfExit(); 

} 
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// — 

//    Function:  step_forward 

//    Returns:  None 

//   Parameters:        Number  of  steps  needs  to  be  taken,  initial  position 

//    Summary:  Computes  the  joint  angles  according  to  the  position 

//  of  the  end  effector(foot)  by  using  the  inverse  kinematic 

//  equations  of  the  three  link  planar  manipulator.  The 

//  algorithm  for  the  path  of  the  foot  is  described  in 

Chapter  IV  of  this  thesis  as  "Stepping  Forward  Algorithm' 
// - 


void  step_forward(int  number_of_steps,float  X,float  Y,float  Z){ 

float  11  =  l.Of; 
float  12=  l.Of; 
float  cl_left,cl_right; 
float  thetal.theta2; 
float  kl_left,k2_left; 
float  kl_right,k2_right; 

float  x_left=    1.732f; 
float  y_left=    O.Of; 
float  x_right  =  1.732f; 
float  yright  =  O.Of; 

//     INITIAL  HALF  STEP 
for  (float  z  =  32  ;  z  <  43  ;  z  +=  0.5  *  deltaj) 

{ 
/*  Go  to  sleep  until  next  frame  time.  */ 
pfSync(); 
Shared->simTime  =  pfGetTime(); 

/*     Main  Body     */ 
dcsO->setRot(90.0,  90.0  ,  0); 
dcsO->setTrans(X,Y,Z); 

/*     Head     */ 
dcs7->setTrans(0.0  ,  0.4  ,  0.0); 

/*     Torso     */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3  ,2.1,-0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3  ,2.1.0.005); 

/*  Left  foot     */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0,  0  .  180.0); 
dcs9->setTrans(0  ,-1.7,0); 

/*  Right  foot     */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0,  0 ,  180.0); 
dcs8->setTrans(0  .-1.7,0); 
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/*  Left  lowerleg     */ 
dcs4->setTrans(0  ,-1.7,0); 

/*  Left  upperleg     */ 
dcs5->setTrans(0.6,  0,    0); 

/*  Right  lowerleg     */ 
dcsl->setTrans(0  ,  -1.7,  0); 

/*  Right  upper  arm  */ 
dcsll->setScale(0.25f); 
dcsll->sefTrans(-1.0,  1.7,0); 

/*  Right  lower  arm  */ 
dcsl2->setTrans(0.4 ,  -4.6,  0.0); 

/*  Right  hand     */ 
dcsl3->setTrans(0.0 ,  -4.2,  0.0); 
dcsl3->setRot(0,  0,  180.0); 

/*  Left  upper  arm     */ 
dcsl4->setScale(0.25f); 
dcsl4->setTrans(1.6  ,  1.8,0); 

/*  Left  lower  arm     */ 
dcsl5->setRot(0,  0,  180.0); 
dcsl5->setTrans(-0.5  ,  -4.6,  0.0); 

/*  Left  hand     */ 
dcsl6->setTrans(-0.2  ,  -4.6,  0.0); 

llllllllllllllllllllllllllllllllllllllllllllllllllllllllllll 

II  Inverse  Kinematics 

// 

llllllllllllllllllllllllllllllllllllllllllllllllllllllllllll 

II     SECOND  STEP 

//     RIGHT  LEG  STEP 

x_right  =  0.95f  *  cos((64  -  2*  z)  *  DEG_TO_RAD  )* 

(11  +  12)  *  cos((64  -  2*  z)  *  DEG_TO_RAD); 

y_right  =  0.95f  *  cos((64  -  2*  z)  *  DEG_TO_RAD  )* 
(11  +  12)  *  sin((64  -  2*  z)  *  DEG_TO_RAD); 

cl_right=  ((x_right*x_right)  +  (y_right*y_right) 

-  (11*11)  -  (12*12))/(2  *  11  *12); 
theta2  =  (1  *acos(cl_right)); 

kl_right  =  11  +  (12  *  cos(theta2)); 
k2_right  =  12  *  sin(theta2); 

thetal  =  atan(y_right/x_right)  -  atan(k2_right/kl_right); 

//  Right  Leg 

dcs2->setRot(0  ,(57.3  *  thetal ),0  ); 

dcsl->setRot(0  ,(57.3  *  theta2),  0); 
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//  Left  Arm 

dcsl4->setRot(0  ,(0.7f  *(57.3  *  thetal)  +  10.0f),0  ); 

dcsl5->setRot(0  ,(1.5f  *(57.3  *  thetal)  -  lO.Of),  0); 

//    LEFT  LEG  STEP 

xjeft  =  cos((-64  +  2*  z)  *  DEG_TO_RAD  )  *  (11  +  12)  * 

cos((-64  +  2*  z)  *  DEG_TO_RAD); 
yjeft  =  cos((-64  +  2*  z)  *  DEG_TO_RAD  )  *  (11  +  12)  * 

sin((-64  +  2*  z)  *  DEG_TO_RAD); 
cl_left=  ((x_left*x_Ieft)  +  (y_left*y_left) 

-  (11*11)  -  (12*12))/(2  *  11  *12); 
theta2  =  (l  *acos(cl_left)); 

kl_left  =  ll  +  (12  *  cos(theta2)); 

k2_left  =  12  *  sin(theta2); 

thetal  =  atan(y_left/x_left)  -  atan(k2_left/kl_left); 

//  Left  leg 

dcs5->setRot(0  ,(57.3  *  thetal),  0); 

dcs4->setRot(0  ,(57.3  *  theta2),  0); 

//  Right  arm 

dcsll->setRot(0,(0.7f  *(57.3  *  thetal)  +  10.0f),0); 

dcsl2->setRot(0,(1.5f  *(57.3  *  thetal)-  10.0f),0); 

//  Torso  rotation 
if(z<31){ 

dcs3->setRot((-(z  -  21)/3.0),0  ,0); 

dcs6->setRot((-(z  -21)/3.0),0  ,0); 

} 
else{ 

dcs3->setRot(((-20  +  ((z-  21)))/3.0),0  ,0); 

dcs6->setRot(((-20  +  ((z-  21)))/3.0),0  ,0); 

} 

dcsO->setTrans(X  +  (z  -  32)  *  0.1  ,  Y  , 

(Z  +  0.2)  +  0.08f  *  cos(2.0f  *  3.14159f  *  (z  -  41)  /  21.00); 

UpdateView(); 
UpdateGUK); 
pfFrame(); 
}//End  of  second  for 

for(  i  =  1  ;  i  <  number_of_steps  ;  i++){ 

for(z  =  (i-l)*42+  1  ;z<(i-l)*42  +  22;z+=0.5  *  delta_t){ 

/*  Go  to  sleep  until  next  frame  time.  */ 

pfSync(); 

Shared->simTime=pfGetTime(); 


/*     Main  Body     */ 
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dcsO->setRot(90.0,  90.0 , 0); 
dcsO->setTrans(X,Y,Z); 

/*     Head     */ 
dcs7->setTrans(0.0  ,  0.4  ,  0.0); 

/*     Torso     */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3  ,2.1,-0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3  ,2.1,0.005); 

/*  Left  foot     */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0,  0  ,  180.0); 
dcs9->setTrans(0  ,-1.7,0); 

/*  Right  foot     */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0,  0  ,  180.0); 
dcs8->setTrans(0  ,-1.7,0); 

/*  Left  lowerleg     */ 
dcs4->setTrans(0  ,  -1.7,  0); 

/*  Left  upperleg     */ 
dcs5->setTrans(0.6,  0,    0); 

/*  Right  lowerleg     */ 
dcsl->setTrans(0  ,-1.7,0); 

/*  Right  upper  arm  */ 
dcsll->setScale(0.25f); 
dcsl  l->setTrans(-1.0  ,  1.7,  0); 

/*  Right  lower  arm  */ 
dcsl2->setTrans(0.4  ,  -4.6,  0.0); 

/*  Right  hand     */ 
dcsl3->setTrans(0.0  ,  -4.2,  0.0); 
dcsl3->setRot(0,0,  180.0); 

/*  Left  upper  arm     */ 
dcsl4->setScale(0.25f); 
dcsl4->setTrans(1.6  ,1.8,0); 

/*  Left  lower  arm     */ 
dcsl5->setRot(0,  0,  180.0); 
dcsl5->setTrans(-0.5  ,  -4.6,  0.0); 

/*  Left  hand     */ 
dcsl6->setTrans(-0.2  ,  -4.6,  0.0); 
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IIIIUIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIHIIIIII 

II  Inverse  Kinematics 

llllllllllllllllllllllllllllllllllllllllllllllllllllllll 
II    FIRST  STEP 
//     LEFT  LEG 

xjeft  =  0.95f  *  cos((22  -  2*  (z-  42  *(i  -  1)))  *  DEG_TO_RAD  ) 

*(11  +  12)  *  cos((22  -  2*  (z-  42  *(i  -  1)))  *  DEG_TO_RAD), 

yjeft  =  0.95f  *  cos((22  -  2*  (z-  42  *(i  -  1)))  *  DEG_TO_RAD  ) 

*(11  +  12)  *  sin((22  -  2*  (z-  42  *(i  -  1)))  *  DEG_TO_RAD); 

cl_left=  ((x_left*x_left)  +  (y_left*y_left)  -  (11*11) 

-  (12*12))/(2  *  11  *12); 
theta2  =  (1  *acos(cl_left)); 
kljeft  =  11  +  (12  *  cos(theta2)); 
k2_left  =  12  *  sin(theta2); 
thetal  =  atan(y_left/x_left)  -  atan(k2_left/kl_left); 

dcs5->setRot(0  ,(57.3  *  thetal ),0  ); 
dcs4->setRot(0  ,(57.3  *  theta2) ,  0); 

//  Right  Arm 

dcsl  l->setRot(0  ,(0.7f  *(57.3  *  thetal)  +  10.0f),0  ); 

dcsl2->setRot(0  ,(1.5f  *(57.3  *  thetal)  -  10.0f),0); 

//  Right  Leg 

x_right  =  cos((-22  +  2  *  (z-  42  *(i  -  1)))  *  DEG_TO_RAD  ) 

*  (11  +  12)  *  cos((-22  +  2  *  (z-  42  *(i  -  1)))  *  DEG_TO_RAD); 
y_right  =  cos((-22  +  2  *  (z-  42  *(i  -  1)))  *  DEG_TO_RAD  ) 

*  (11  +  12)  *  sin((-22  +  2  *  (z-  42  *(i  -  1)))  *  DEG_TO_RAD); 
cl_right=  ((x_right*x_right)  +  (y_right*y_right) 

-  (11*11) -(12*12))/(2*  11  *12); 
theta2  =  (1  *acos(cl_right)); 

kl_right  =  11  +  (12  *  cos(theta2)); 

k2_right  =  12  *  sin(theta2); 

thetal  =  atan(y_right/x_right)  -  atan(k2_right/kl_right); 

dcs2->setRot(0  ,(57.3  *  thetal),  0); 
dcsl->setRot(0  ,(57.3  *  theta2),  0); 

//  Left  Arm 

dcsl4->setRot(0  ,(0.7f  *(57.3  *  thetal)  +  10.00,0  ); 

dcs  1 5->setRot(0  ,( 1 .5f  *(57.3  *  thetal )  -  1 0.Of).  0); 

//  Torso  rotation 
if((z- 42  *(i  -  1))<  11){ 

dcs3->setRot(((z-  42  *(i  -  l))/3.0),0  ,0); 

dcs6->setRot(((z-  42  *(i  -  l))/3.0),0  ,0); 

} 
else{ 

dcs3->setRot(((20  -  (z-  42  *(i  -  l)))/3.0),0  ,0); 

dcs6->setRot(((20  -  (z-  42  *(i  -  l)))/3.0),0  ,0); 
} 

dcsO->setTrans(  X  +  1 .0  +  z  *  0. 1   .  Y  ,  Z  +  0.2  +  0.08f 
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*cos(2.0f  *  3. 14159f  *  z  /  21. Of)); 


UpdateView(); 

UpdateGUIO; 

pfFrame(); 

} 

for  (  z  =  (i-l)*42  +  22  ;  z  <  (i-l)*42  +  43  ;  z  +=  0.5  *  delta_t){ 

/*  Go  to  sleep  until  next  frame  time.  */ 

pfSyncO; 

Shared->simTime  =  pfGetTime(); 

/*     Main  Body     */ 
dcsO->setRot(90.0,  90.0 , 0); 
dcsO->setTrans(X,Y,Z); 

/*     Head     */ 
dcs7->setTrans(0.0  ,  0.4  ,  0.0); 

/*     Torso     */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3  ,2.1,-0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3  ,2.1,0.005); 

/*  Left  foot     */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0,  0 ,  180.0); 
dcs9->setTrans(0  ,-1.7,0); 

/*  Right  foot     */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0,  0  ,  180.0); 
dcs8->setTrans(0  ,-1.7,0); 

/*  Left  lowerleg     */ 
dcs4->setTrans(0  ,-1.7,0); 

/*  Left  upperleg     */ 
dcs5->setTrans(0.6,  0,    0); 

/*  Right  lowerleg     */ 
dcsl->setTrans(0  ,-1.7,0); 

/*  Right  upper  arm  */ 
dcsll->setScale(0.25f); 
dcsll->setTrans(-1.0  ,  1.7,0); 

/*  Right  lower  arm  */ 
dcsl2->setTrans(0.4  ,  -4.6,  0.0); 

/*  Right  hand     */ 
dcsl3->setTrans(0.0  .  -4.2,  0.0); 
dcsl3->setRot(0.  0,  180.0); 
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/*  Left  upper  arm     */ 
dcsl4->setScale(0.25f); 
dcsl4->setTrans(1.6  ,1.8,0); 

/*  Left  lower  arm     */ 
dcsl5->setRot(0,  0,  180.0); 
dcsl5->setTrans(-0.5  ,  -4.6,  0.0); 

/*  Left  hand     */ 
dcsl6->setTrans(-0.2  ,  -4.6,  0.0); 

iiiiiiiiiiiiiiiiiiiiiiiiimiiiiiiiiiiiiiiiiiiiiiiiiiiii 

II  Inverse  Kinematics 

llllllllllllllllllllllllllllllllllllllllllllllllllllllll 


llllllllllllllllllllllllllllllllllllllllllllllllllllllll 
II    SECOND  STEP 
//    RIGHT  LEG  STEP 


x_right  =  0.95f  *  cos((64  -  2*  (z-  42  *(i  -  1)))  *  DEG_TO_RAD  ) 

*  (11  +  12)  *  cos((64  -  2*  (z-  42  *(i  -  1)))  *  DEG_TO_RAD); 
yjright  =  0.95f  *  cos((64  -  2*  (z-  42  *(i  -  1 )))  *  DEG_TO_RAD  ) 

*  (11  +  12)  *  sin((64  -  2*  (z-  42  *(i  -  1)))  *  DEG_TO_RAD); 

cl_right=  ((x_right*x_right)  +  (y_nght*y_right) 

-  (11*11)  -  (12*12))/(2  *  11  *12); 
theta2  =  (l  *acos(cl_right)); 

kl_right  =  11  +  (12  *  cos(theta2)); 
k2_right  =  12  *  sin(theta2); 

thetal  =  atan(y_right/x_right)  -  atan(k2_right/kl_right); 

//  Right  Leg 

dcs2->setRot(0  ,(57.3  *  thetal  ),0  ); 

dcsl->setRot(C  ,(57.3  *  theta2),  0); 

//  Left  ARM 

dcsl4->setRot(0  ,(0.7f  *(57.3  *  thetal)  +  10.0f),0): 

dcsl5->setRot(0  ,(1.5f  *(57.3  *  thetal)  -  10.00,0); 

//    LEFT  LEG  STEP 

xjeft  =  cos((-64  +  2*  (z-  42  *(i  -  1)))  *  DEG_TO_RAD  ) 

*  (11  +  12)  *  cos((-64  +  2*  (z-  42  *(i  -  1)))  *  DEG_TO_RAD); 
yjeft  =  cos((-64  +  2*  (z-  42  *(i  -  1)))  *  DEG_TO_RAD  ) 

*  (11  +  12)  *  sin((-64  +  2*  (z-  42  *(i  -  1)))  *  DEG_TO_RAD); 

cl_left=  ((x_left*x_left)  +  (y_left*y_left)  -  (11*11) 
-  (12*12))/(2  *  11  *12); 

theta2  =  (l  *acos(cl_left)); 

kljeft  =  11  +  (12  *  cos(theta2)); 

k2_left  =  12  *  sin(theta2); 

thetal-=  atan(y_left/x_left)  -  atan(k2_left/kl_left); 
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dcs5->setRot(0  ,(57.3  *  thetal),  0); 
dcs4->setRot(0  ,(57.3  *  theta2),  0); 

//  Right  arm 

dcsll->setRot(0  ,(0.7f  *(57.3  *  thetal)  +  10.0f),0  ); 

dcsl2->setRot(0  ,(1.5f  *(57.3  *  thetal)  -  lO.Of),  0); 

//  Torso  rotation 
if((z-42*(i-  1))<31){ 

dcs3->setRot((-(z-  42  *(i  -  1)  -  21)/3.0),0  ,0); 

dcs6->setRot((-(z-  42  *(i  -  1)  -21)/3.0),0  ,0); 

} 
else{ 

dcs3->setRot(((-20  +  ((z-  42  *(i  -  1)-  21)))/3.0),0  ,0); 

dcs6->setRot(((-20  +  ((z-  42  *(i  -  1)-  21)))/3.0),0  ,0); 


dcsO->setTrans(X  +  1.0  +  z  *  0.1   ,  Y  ,  Z  +  0.2  +  0.08f 

*  cos(2.0f  *  3. 14159f  *  z  /  21. Of)); 
UpdateView(); 
UpdateGUK); 
pfFrameO; 
} 
}//Big  FOR 

//    LAST  HALF  STEP ///////// 

for  (  z  =  1  ;  z  <  12  ;  z  +=  0.5  *  delta_t){ 

/*  Go  to  sleep  until  next  frame  time.  */ 

pfSync(); 

Shared->simTime=pfGetTime(); 

/*     Main  Body     */ 

dcs0->setRot(90.0,  90.0  , 0); 
dcs0->setTrans(X,Y,Z); 

/*     Head     */ 
dcs7->setTrans(0.0  ,  0.4  ,  0.0); 

/*     Torso     */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3  ,2.1,-0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3  ,2.1,0.005); 

/*  Left  foot     */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0,  0  ,  180.0); 
dcs9->setTrans(0  ,-1.7,0); 

/*  Right  foot     */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0,  0  ,  180.0); 
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dcs8->setTrans(0  ,-1.7,0); 

/*  Left  lowerleg     */ 
dcs4->setTrans(0  ,-1.7,0); 

/*  Left  upperleg     */ 
dcs5->setTrans(0.6,  0,    0); 

/*  Right  lowerleg     */ 
dcsl->setTrans(0  ,-1.7,0); 

/*  Right  upper  arm  */ 
dcsll->setScale(0.25f); 
dcsll->sefTrans(-1.0,  1.7,0); 

/*  Right  lower  arm  */ 
dcsl2->setTrans(0.4  ,  -4.6,  0.0); 

/*  Right  hand     */ 
dcsl3->setTrans(0.0  ,  -4.2,  0.0); 
dcsl3->setRot(0,  0  ,  180.0); 

/*  Left  upper  arm     */ 
dcsl4->setScale(0.25f); 
dcsl4->setTrans(1.6  ,1.8,0); 

/*  Left  lower  arm     */ 
dcsl5->setRot(0,  0,  180.0); 
dcsl5->setTrans(-0.5  ,  -4.6,  0.0); 

/*  Left  hand     */ 
dcsl6->setTrans(-0.2  ,  -4.6,  0.0); 

IIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIHIIIIHIIIII 
II  Inverse  Kinematics 

llllllllllllllllllllllllllllllllllllllllllllllllllllllll 
II    LEFT  LEG 

xjeft  =  0.95f  *  cos((22  -  2*  z)  *  DEG_TO_RAD  )  *(11  +  12) 

*  cos((22  -  2*  z)  *  DEG_TO_RAD); 

y_left  =  0.95f  *  cos((22  -  2*  z)  *  DEG_TO_RAD  )  *(11  +  12) 

*  sin((22  -  2*  z)  *  DEG_TO_RAD); 

cl_left=  ((x_left*x_left)  +  (y_left*y_left) 
-  (11*11)  -  (12*12))/(2  *  11  *12); 

theta2  =  (1  *acos(cl_left)); 

kljeft  =  11  +  (12  *  cos(theta2)); 

k2_left  =  12  *  sin(theta2); 

thetal  =  atan(y_left/x_left)  -  atan(k2_left/kl_left); 

dcs5->setRot(0  ,(57.3  *  thetal ),0  ); 
dcs4->setRot(0  ,(57.3  *  theta2) ,  0); 
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} 


//  Right  arm 

dcsl  l->setRot(0  ,(0.7f  *(57.3  *  thetal)  +  10.00,0 ); 

dcsl2->setRot(0  ,(1.5f  *(57.3  *  thetal)  -  10.00,  0); 

//    RIGHT  LEG 

x_right  =  cos((-22  +  2  *  z)  *  DEG_TO_RAD  )  *  (11  +  12) 

*  cos((-22  +  2  *  z)  *  DEG_TO_RAD); 

y_right  =  cos((-22  +  2  *  z)  *  DEG_TO_RAD  )  *  (11  +  12) 

*  sin((-22  +  2  *  z)  *  DEG_TO_RAD); 
cl_right=  ((x_right*x_right)  +  (y_right*y_right)  -  (11*11) 

-  (12*12))/(2  *  11  *12); 
theta2  =  (l  *acos(cl_right)); 

kl_right  =  11  +  (12  *  cos(theta2)); 

k2_right  =  12  *  sin(theta2); 

thetal  =  atan(y_right/x_right)  -  atan(k2_right/kl_right); 

dcs2->setRot(0  ,(57.3  *  thetal),  0); 
dcsl->setRot(0  ,(57.3  *  theta2),  0); 

//  Left  arm 

dcsl4->setRot(0  ,(0.7f  *(57.3  *  thetal)  +  10.00,0  ); 

dcsl5->setRot(0  ,(1.5f  *(57.3  *  thetal)  -  10.00,  0); 

//  TORSO  rotation 

if(z<  11){ 

dcs3->setRot((z/3.0),0  ,0); 

dcs6->setRot((z/3.0),0  ,0); 

} 
else{ 

dcs3->setRot(((20  -  z)/3.0),0  ,0); 

dcs6->setRot(((20  -  z)/3.0),0  ,0); 

} 

dcsO->setTrans(X  +  1.0  +  (i-1)  *  4.2  +  z  *  0.1 

,  Y  ,  +Z  +  0.2  +  0.08f  *cos(2.0f  *  3.14159f  *  z  /  21.00); 

UpdateView(); 
UpdateGUK); 
pfFrame(); 
}//  END  of  step_forward 
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// 

//  Function:  step_upward 

//  Returns:  None 

//  Parameters:         Number  of  steps  needs  to  be  taken,  initial  position 

//  Summary:  Computes  the  joint  angles  according  to  the  position 

//  of  the  end  effector(foot)  by  using  the  inverse  kinematic 

//  equations  of  the  three  link  planar  manipulator.  The 

//  algorithm  for  the  path  of  the  foot  is  described  in 

//  Chapter  IV  of  this  thesis  as  "Stepping  Upward  Algorithm. 

//  Height  of  each  step  is  0.267949  units. 

//-- 

void  step_upward(int  number_of_steps,float  X.float  Y, float  Z){ 


float  11  =  l.Of; 
float  12  =  1. Of; 
float  cl_left,cl_right; 
float  thetal,theta2; 
float  kl_left,k2_left; 
float  kl_right,k2_right; 

float  x_left=    1.732f; 
float  y_left=   O.Of; 
float  x_right  =  1.732f; 
float  y_right  =  O.Of; 
float    z; 

lllllllllllllllllllllllllllllllllllll 
II     FIRST  HALF  STEP 
lllllllllllllllllllllllllllllllllll 

for  (  z  =  1 1 .0  ;  z  <  22.0  ;  z  +=  0.3  *  delta.t) 

{ 

/*  Go  to  sleep  until  next  frame  time.  */ 

pfSync(); 

Shared->simTime=pfGetTime(); 

/*     Main  Body     */ 
dcsO->setRot(90.0,  90.0  ,  0.0); 
dcsO->setTrans(X,Y,Z); 

/*     Head     */ 
dcs7->setTrans(0.0  ,  0.4  ,  0.0); 

/*     Torso     */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3  ,2.1,-0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3  ,2.1,0.005); 


/*  Left  foot     */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0,  0  ,  180.0): 
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dcs9->setTrans(0  ,-1.7,0); 

/*  Right  foot     */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0,  0 ,  180.0); 
dcs8->setTrans(0  ,-1.7,0); 

/*  Left  lowerleg     */ 
dcs4->setTrans(0  ,-1.7,0); 

/*  Left  upperleg     */ 
dcs5->setTrans(0.6,  0,    0); 

/*  Right  lowerleg     */ 
dcsl->setTrans(0  ,-1.7,0); 

/*  Right  upper  arm     */ 
dcsll->setScale(0.25f); 

/*  Right  lower  arm     */ 
dcsl2->setTrans(0.4  ,  -4.6,  0.0); 

/*  Right  hand     */ 
dcsl3->setTrans(0.0  ,  -4.2,  0.0); 
dcsl3->setRot(0,  0,  180.0); 

/*  Left  upper  arm     */ 
dcsl4->setScale(0.25f); 
//dcsl4->setTrans(1.6  ,1.8,0); 

/*  Left  lower  arm     */ 
dcsl5->setRot(0,  0,  180.0); 
dcsl5->sefTrans(-0.5  ,  -4.6,  0.0); 

/*  Left  hand     */ 
dcsl6->setTrans(-0.2 ,  -4.6,  0.0); 

llllllllllllllllllllllllllllllllllllllllllllllllllllllll 
II  Inverse  Kinematics 

llllllllllllllllllllllllllllllllllllllllllllllllllllllll 
II   FIRST  STEP 
//     LEFT  LEG 

xjeft  =  0.95f  *  cos((22  -  2*  z  -  10)  *  DEG_TO_RAD  )  *(11  +  12) 

*  cos((22  -  2*  z  -  10)  *  DEG_TO_RAD); 

yjeft  =  0.95f  *  cos((22  -  2*  z  -  10)  *  DEG_TO_RAD  )  *(11  +  12) 

*  sin((22  -  2*  z  -  10)  *  DEG_TO_RAD); 

cl_left=  ((x_left*x_left)  +  (y_left*y_left)  -  (11*11) 
-  (12*12))/(2  *  11  *12); 

theta2  =  (1  *acos(cl_left)); 

kljeft  =  11  +  (12  *  cos(theta2)); 

k2_left  =  12  *  sin(theta2); 

thetal  =  atan(y_left/x_left)  -  atan(k2_left/kl_left); 
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dcs5->setRot(0  ,(57.3  *  thetal),0  ); 
dcs4->setRot(0  ,(57.3  *  theta2)  ,  0); 

//  Right  arm 

dcsl  l->setTrans(-1.0  .  1.7,  0.5); 

dcsl  l->setRot(0  ,(0.7f  *(57.3  *  thetal)  +  10.00,0  ); 

dcs  1 2->setRot(0  ,(1.5f  *(57.3  *  thetal )  -  1 0.Of),  0); 

RIGHT  LEG 

x.right  =  cos((-22  +  2  *  z  -  10)  *  DEG_TO_RAD  )  *  (11  +  12) 

*  cos((-22  +  2  *  z  -  10)  *  DEG_TO_RAD); 

y.right  =  cos((-22  +  2  *  z  -  10)  *  DEG_TO_RAD  )  *  (11  +  12) 

*  sin((-22  +  2  *  z  -  10)  *  DEG_TO_RAD); 
cl_right=  ((x_right*x_right)  +  (y_right*y_right)  -  (11*11) 

-  (12*12))/(2  *  11  *12); 
theta2  =  (l  *acos(cl_right)); 

kl_right  =  11  +  (12  *  cos(theta2)); 

k2_right  =  12  *  sin(theta2); 

thetal  =  atan(y_right/x_right)  -  atan(k2_right/kl_right); 

dcs2->setRot(0  ,(57.3  *  thetal),  0); 
dcsl->setRot(0  ,(57.3  *  theta2),  0); 
//  Left  arm 

dcsl4->setTrans(1.6  ,1.8,0.5); 
dcsl4->setRot(0  ,(0.7f  *(57.3  *  thetal)  +  10.00,0  ); 
dcsl5->setRot(0,(1.5f  *(57.3  *  thetal)-  10.00,0); 
//  TORSO  rotation 
if(z<  11){ 

dcs3->setTrans(0.3  ,  2.1  ,  0.305); 

dcs6->setTrans(0.3  ,2.1,0.305); 

dcs3->setRot((z/3.0),7.0,0); 

dcs6->setRot((z/3.0),7.0,0); 

} 
else{ 

dcs3->setTrans(0.3  ,2.1,0.305); 

dcs6->setTrans(0.3  ,2.1,0.305); 

dcs3->setRot(((20  -  z)/3.0)  ,7.0,0); 

dcs6->setRot(((20  -  z)/3.0)  ,7.0,0); 

} 

z  =  z+21; 

dcsO->setTrans(  X  +  (z  -  32)  *  0.1  -0.4,  Y  , 

Z  -  2.005063  +  /*  0.08f  *cos(2.0f  *  3.14159f  *  z  /  21.00  */0 

+  1  *  0.267949  *  3.4  *2  +  ((11  +  12) 

-(11  +  12)  *  cos((  -22  +  2  *  z  -  10)  *  DEG_TO_RAD))); 
z  =  z-21; 


UpdateViewQ; 
UpdateGUK); 
pfFrame(); 
}//END  FOR 
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for(  i  =  1  ;  i  <  number_of_steps  ;  i++){ 

for  (  z  =  (i-l)*42  +  22.0  ;  z  <  (i-l)*42  +  43.0  ;  z  +=  0.35  *  delta_t){ 
/*  Go  to  sleep  until  next  frame  time.  */ 
pfSync(); 
Shared->simTime  =  pfGetTime(); 

/*     Main  Body     */ 
dcsO->setRot(90.0,  90.0  , 0.0); 
dcs0->sefTrans(X,Y,Z); 

/*     Head     */ 
dcs7->setTrans(0.0  ,  0.4  ,  0.0); 

/*     Torso     */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3  ,2.1,-0.005); 
dcs3->setScale(2.0f); 

dcs3->setTrans(0.3  ,2.1,0.005); 

/*  Left  foot     */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0,  0 ,  180.0); 
dcs9->setTrans(0  ,  -1.7,  0); 

/*  Right  foot     */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0,  0  ,  180.0); 
dcs8->setTrans(0  ,  -1.7,  0); 

/*  Left  lowerleg     */ 
dcs4->setTrans(0  ,-1.7,0); 

/*  Left  upperleg     */ 
dcs5->setTrans(0.6,  0,    0); 

/*  Right  lowerleg     */ 
dcsl->setTrans(0  ,-1.7,0); 

/*  Right  upper  arm     */ 
dcsll->setScale(0.25f); 

/*  Right  lower  arm     */ 
dcsl2->setTrans(0.4  ,  -4.6,  0.0); 

/*  Right  hand     */ 
dcsl3->setTrans(0.0 ,  -4.2,  0.0); 
dcsl3->setRot(0,  0,  180.0); 

/*  Left  upper  arm     */ 
dcsl4->setScale(0.25f): 

/*  Left  lower  arm     */ 
dcsl5.->setRot(0,  0  ,  180.0); 
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dcsl5->setTrans(-0.5  ,  -4.6,  0.0); 

/*  Left  hand     */ 
dcsl6->setTrans(-0.2  ,  -4.6,  0.0); 

IIIIIIIIIIIIIIUIimillllllllllUIIIIIIIIIIIIIIIHIIIII 
II  Inverse  Kinematics 

llllllllllllllllllllllllllllllllllllllllllllllllllllllll 


llllllllllllllllllllllllllllllllllllllllllllllllllllllllllll 
II    SECOND  STEP 
//     RIGHT  LEG  STEP 

x_right  =  0.95f  *  cos((64  -  2*  (z-  42  *(i  -  1))  -  10)  *  DEG_TO_RAD) 

*  (11  +  12)  *  cos((64  -  2*  (z-  42  *(i  -  1))  -  10) 

*  DEG_TO_RAD); 

y_right  =  0.95f  *  cos((64  -  2*  (z-  42  *(i  -  1))  -  10)  *  DEG_TO_RAD) 

*  (11  +  12)  *  sin((64  -  2*  (z-  42  *(i  -  1))  -  10) 

*  DEG_TO_RAD); 

cl_right=  ((x_right*x_right)  +  (y_right*y_right)  -  (11*11) 

-  (12*12))/(2  *  11  *12); 
theta2  =  (l  *acos(cl_right)); 

kl_right  =  11  +  (12  *  cos(theta2)); 

k2_nght  =  12  *  sin(theta2); 

thetal  =  atan(y_right/x_right)  -  atan(k2_right/kl_right); 

//  Right  LEG 

dcs2->setRot(0  ,(57.3  *  thetal ),0  ); 

dcsl->setRot(0  ,(57.3  *  theta2),  0); 

//  Left  arm 

dcsl4->setTrans(1.6  ,1.8,0.5); 
dcsl4->setRot(0  ,(0.7f  *(57.3  *  thetal)  +  10.0f),0  ); 
dcsl5->setRot(0,(1.5f*(57.3  *  thetal)-  10.00,0); 


//     LEFT  LEG  STEP 

xjeft  =  cos((-64  +  2*  (z-  42  *(i  -  1))  -  10)  *  DEG_TO_RAD  ) 
*(11  +12) 

*  cos((-64  +  2*  (z-  42  *(i-  1))  -  10)  *  DEG_TO_RAD); 
yjeft  =  cos((-64  +  2*  (z-  42  *(i  -  1))  -  10)  *  DEG_TO_RAD  ) 

*(11  +12) 

*  sin((-64  +  2*  (z-  42  *(i  -  1))  -  10)  *  DEG_TO_RAD); 

cl_left=  ((x_left*x_left)  +  (y_left*y_left)  -  (11*11) 

-(12*12))/(2*11  *12); 
theta2  =  (l  *acos(cl_left)); 

kljeft  =  11  +  (12  *  cos(theta2)); 

k2_left  =  12  *  sin(theta2); 

thetal  =  atan(yjeft/x_left)  -  atan(k2_left/kl_left); 

dcs5->setRot(0  ,(57.3  *  thetal),  0); 
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dcs4->setRot(0  ,(57.3  *  theta2),  0); 

//  Right  arm 

dcsll->setTrans(-1.0,  1.7,0.5); 

dcsl  l->setRot(0  ,(0.7f  *(57.3  *  thetal)  +  10.0f),0  ); 

dcsl2->setRot(0  ,(1.5f  *(57.3  *  thetal)  -  lO.Of),  0); 

//  TORSO  rotation 
if((z-42*(i-  1))<31){ 

dcs3->setTrans(0.3  ,2.1,0.305); 

dcs6->setTrans(0.3  ,2.1,0.305); 

dcs3->setRot((-(z-  42  *(i  -  1)  -  21)/3.0)  ,7.0,0); 

dcs6->setRot((-(z-  42  *(i  -  1)  -21)/3.0)  ,7.0,0); 

} 

else{ 

dcs3->setTrans(0.3  ,2.1,0.305); 
dcs6->setTrans(0.3  ,  2.1  ,  0.305); 
dcs3->setRot(((-20  +  ((z-  42  *(i  -  1)-  21)))/3.0)  ,7.0,0); 
dcs6->setRot(((-20  +  ((z-  42  *(i  -  1)-  21)))/3.0)  ,7.0,0); 
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z  =  z-21; 

dcs0->setTrans(  X  +  0.5  +  z*0.1  ,Y,Z 
+  i  *  0.267949  *  3.4  *2  -  ((11  +  12)  -  (11  +  12) 
*  cos((  64  -  2  *  (z-  42  *(i  -  1)) )  *  DEG_TO_RAD))); 

z  =  z+21; 

UpdateView(); 
UpdateGUIO; 

pfFrameO; 


for  ( z  =  (i- 1  )*42  +  1 .0  ;  z  <  (i- 1  )*42  +  22.0  ;  z  +=  0.35  *  delta_t)  { 

/*  Go  to  sleep  until  next  frame  time.  */ 

pfSync(); 

Shared->simTime=pfGetTime(); 

/*     Main  Body     */ 

dcs0->setRot(90.0,  90.0  ,0); 
dcsO->setTrans(X,Y,Z); 

/*     Head     */ 
dcs7->setTrans(0.0  ,  0.4  ,  0.0); 

/*     Torso     */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3  ,2.1,-0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3  ,2.1,0.005); 
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/*  Left  foot     */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0,  0  ,  180.0); 
dcs9->setTrans(0  ,-1.7,0); 

/*  Right  foot     */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0,  0  ,  180.0); 
dcs8->setTrans(0  ,-1.7,0); 

/*  Left  lowerleg     */ 
dcs4->setTrans(0  ,-1.7,0); 

/*  Left  upperleg     */ 
dcs5->setTrans(0.6,  0,    0); 

/*  Right  lowerleg     */ 
dcsl->setTrans(0  ,-1.7,0); 

/*  Right  upper  arm     */ 
dcsll->setScale(0.25f); 

/*  Right  lower  arm     */ 
dcsl2->setTrans(0.4 ,  -4.6,  0.0); 

I*  Right  hand     */ 
dcsl3->setTrans(0.0  ,  -4.2,  0.0); 
dcsl3->setRot(0,  0  ,  180.0); 

/*  Left  upper  arm     */ 
dcsl4->setScale(0.25f); 

/*  Left  lower  arm     */ 
dcsl5->setRot(0,  0,  180.0); 
dcsl5->setTrans(-0.5  ,  -4.6,  0.0); 

/*  Left  hand     */ 
dcsl6->setTrans(-0.2  ,  -4.6,  0.0); 

llllllllllllllllllllllllllllllllllllllllllllllllllllllll 
II  Inverse  Kinematics 

llllllllllllllllllllllllllllllllllllllllllllllllllllllll 
II    FIRST  STEP 
//     LEFT  LEG 

xjeft  =  0.95f  *  cos((22  -  2*  (z-  42  *(i  -  1))  -  10)  *  DEG_TO_RAD  ) 

*(11  +  12)  *  cos((22  -  2*  (z-  42  *(i  -  1))  -  10)  *DEG_TO_RAD); 
yjeft  =  0.95f  *  cos((22  -  2*  (z-  42  *(i  -  1))  -  10)  *  DEG_TO_RAD  ) 
*(11  +  12)  *  sin((22  -  2*  (z-  42  *(i  -  1))  -  10)  *DEG_TO_RAD); 
cl_left=  ((x_left*x_left)  +  (y_left*y_left)  -  (11*11) 
-  (12*12))/(2  *  11  *12); 

theta2  =  (1  *acos(cl_left)); 

kljeft  =  11  +  (12  *  cos(theta2)); 
k2_left  =  12  *  sin(theta2); 


143 


thetal  =  atan(y_left/x_left)  -  atan(k2_left/kl_left); 

dcs5->setRot(0  ,(57.3  *  thetal ),0  ); 
dcs4->setRot(0  ,(57.3  *  theta2) ,  0); 

//  Right  arm 

dcsll->setTrans(-1.0,  1.7,0.5); 
dcsll->setRot(0  ,(0.7f  *(57.3  *  thetal)  +  10.0f),0  ); 
dcsl2->setRot(0  ,(1.5f  *(57.3  *  thetal)  -  10.00,  0); 

//     RIGHT  LEG 

x.right  =  cos((-22  +  2  *  (z-  42  *(i  -  1))  -  10)  *  DEG_TO_RAD  ) 

*  (11  +  12)  *  cos((-22  +  2  *  (z-  42  *(i  -  1))  -  10) 

*  DEG_TO_RAD); 

y  .right  =  cos((-22  +  2  *  (z-  42  *(i  -  1))  -  10)  *  DEG_TO_RAD  ) 

*  (11  +  12)  *  sin((-22  +  2  *  (z-  42  *(i  -  1))  -  10) 

*  DEG_TO_RAD); 

cl_right=  ((x_right*x_right)  +  (y_right*y_right)  -  (11*11) 

-(12*12))/(2*11  *12); 
theta2  =  (l  *acos(cl_right)); 

kl_right  =  11  +  (12  *  cos(theta2)); 

k2_right  =  12  *  sin(theta2); 

thetal  =  atan(y_right/x_right)  -  atan(k2_right/kl_right); 

dcs2->setRot(0  ,(57.3  *  thetal),  0); 
dcsl->setRot(0  ,(57.3  *  theta2),  0); 
//  Left  arm 

dcsl4->setTrans(1.6  ,1.8,0.5); 
dcsl4->setRot(0  ,(0.7f  *(57.3  *  thetal)  +  10.0f),0  ); 
dcsl5->setRot(0  ,(1.5f  *(57.3  *  thetal)  -  10.00,  0); 
//  TORSO  rotation 
if((z-42*(i-  1))<  11){ 

dcs3->setTrans(0.3  ,2.1,0.305); 

dcs6->setTrans(0.3  ,2.1,0.305); 

dcs3->setRot(((z- 42  *(i  -  l))/3.0)  ,7.0  ,0); 

dcs6->setRot(((z-  42  *(i  -  l))/3.0)  ,7.0  ,0); 

} 
else{ 

dcs3->setTrans(0.3  ,2.1,0.305); 

dcs6->setTrans(0.3  ,2.1,0.305); 

dcs3->setRot(((20  -  (z-  42  *(i  -  l)))/3.0)  ,7.0  ,0); 

dcs6->setRot(((20  -  (z-  42  *(i  -  l)))/3.0)  ,7.0  ,0); 

) 

z  =  z+21; 

dcsO->setTrans(X  +  0.5  +  z  *  0. 1   ,  Y  ,  Z  -0. 105327 

+  i  *  0.267949  *  3.4  *2  +  ((11  +  12)  -(11  +  12) 

*  cos((  -22  +  2  *  (z-  42  *(i  -  1))-  10) 

*  DEG_TO_RAD))); 
z  =  z-21; 

UpdateView(); 
UpdateGUK); 
pfFrame(); 
}//END  FOR 
}//BigFOR 
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///////////////////////////////////////////////////// 

//  LAST  HALF 

///////////////////////////////////////////////////// 

for  (  z  =  22.0  ;  z  <  33.0  ;  z  +=  0.35  *  delta_t){ 
/*  Go  to  sleep  until  next  frame  time.  */ 
pfSync(); 
Shared->simTime  =  pfGetTime(); 

/*     Main  Body     */ 
dcsO->setRot(90.0,  90.0  , 0.0); 
dcsO->setTrans(X,  Y  ,Z) ; 

/*     Head     */ 
dcs7->setTrans(0.0  ,  0.4  ,  0.0); 

/*     Torso     */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3  ,2.1,-0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3  ,2.1,0.005); 


/*  Left  foot     */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0,  0  ,  180.0); 
dcs9->setTrans(0  ,-1.7,0); 

/*  Right  foot     */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0,  0  ,  180.0): 
dcs8->setTrans(0  ,-1.7,0); 

/*  Left  lowerleg     */ 
dcs4->setTrans(0  ,-1.7,0); 

/*  Left  upperleg     */ 
dcs5->setTrans(0.6,  0,    0); 

/*  Right  lowerleg     */ 
dcsl->setTrans(0  ,-1.7,0); 

/*  Right  upper  arm     */ 
dcsll->setScale(0.25f); 

/*  Right  lower  arm     */ 
dcsl2->setTrans(0.4  ,  -4.6,  0.0); 

/*  Right  hand     */ 
dcsl3->setTrans(0.0  ,  -4.2,  0.0); 
dcsl3->setRot(0,  0,  180.0); 

/*  Left  upper  arm     */ 
dcsl4->setScale(0.25f); 
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/*  Left  lower  arm     */ 
dcsl5->setRot(0,  0,  180.0); 
dcsl5->setTrans(-0.5  ,  -4.6,  0.0); 

/*  Left  hand     */ 
dcsl6->setTrans(-0.2  ,  -4.6, 0.0); 

//////////////////////////////////////////////////////// 
//  Inverse  Kinematics 

llllllllllllllllllllllllllllllllllllllllllllllllllllllll 


llllllllllllllllllllllllllllllllllllllllllllllllllllllil 
II    SECOND  STEP 
//   RIGHT  LEG  STEP 


x_right  =  0.95f  *  cos((64  -  2*  z  -  10)  *  DEG_TO_RAD  )*  (11  + 12) 

*  cos((64  -  2*  z  -  10)  *  DEG_TO_RAD); 

y_right  =  0.95f  *  cos((64  -  2*  z  -  10)  *  DEG_TO_RAD  )*  (11  +  12) 

*  sin((64  -  2*  z  -  10)  *  DEG_TO_RAD); 

cl_right=  ((x_right*x_right)  +  (y_right*y_right)  -  (11*11) 

-(12*12))/(2*11  *12); 
theta2  =  (l  *acos(cl_right)); 

kl_right  =  11  +  (12  *  cos(theta2)); 
k2_right  =  12  *  sin(theta2); 

thetal  =  atan(y_right/x_right)  -  atan(k2_right/kl_right); 

//  Right  LEG 

dcs2->setRot(0  ,(57.3  *  thetal  ),0  ); 

dcsl->setRot(0  ,(57.3  *  theta2),  0); 

//  Left  arm 

dcsl4->setTrans(1.6  ,1.8,0.5); 
dcsl4->setRot(0  ,(0.7f  *(57.3  *  thetal)  +  10.0f),0  ); 
dcsl5->setRot(0  ,(1.5f  *(57.3  *  thetal)  -  lO.Of),  0); 

//     LEFT  LEG  STEP 

xjeft  =  cos((-64  +  2*  z  -  10)  *  DEG_TO_RAD  )  *  (11  +12) 

*  cos((-64  +  2*  z  -  10)  *  DEG_TO_RAD); 

yjeft  =  cos((-64  +  2*  z  -  10)  *  DEG_TO_RAD  )  *  (11  +  12) 

*  sin((-64  +  2*  z  -  10)  *  DEG_TO_RAD); 

cl_left=  ((x_left*x_left)  +  (y_left*y_left)  -  (11*11) 

-  (12*12))/(2  *  11  *12); 
theta2  =  (1  *acos(cl_left)); 


kljeft  =  11  +  (12  *  cos(theta2)); 

k2_left  =  12  *  sin(theta2); 

thetal  =  atan(y_left/x_left)  -  atan(k2_left/kl_left); 

dcs5->setRot(0  ,(57.3  *  thetal),  0); 
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dcs4->setRot(0  ,(57.3  *  theta2),  0); 
//  Right  arm 

dcsll->setTrans(-1.0,  1.7,0.5); 

dcsl  l->setRot(0  ,(0.7f  *(57.3  *  thetal)  +  10.00.0  ); 

dcsl2->setRot(0  ,(1.5f  *(57.3  *  thetal)  -  10.00,  0); 

//  Torso  rotation 
if(z<31){ 

dcs3->setTrans(0.3  ,2.1,0.305); 

dcs6->setTrans(0.3  ,2.1,0.305); 

dcs3->setRot((-z  -  21)/3.0  +13.75,7.0,0); 

dcs6->setRot((-z  -21)/3.0  +13.75,7.0,0); 

} 
else{ 

dcs3->setTrans(0.3  ,2.1,0.305); 

dcs6->setTrans(0.3  ,2.1,0.305); 

dcs3->setRot((-20+  z-  21)/3.0  ,7.0,0); 

dcs6->setRot((-20  +  z-  21)/3.0  ,7.0,0); 

} 

z  =  z-21; 

dcs0->setTrans(  X  +  0.5  +(i- 1 )  *  4.2+  z  *  0. 1   ,  Y  ,  Z 

+  number_of_steps  *  0.267949  *  3.4  *2  -  ((11  +  12) 
-  (11  +  12)  *  cos((  64  -  2  *  z  )  *  DEG_TO_RAD))); 

z  =  z+21; 

Update  View(); 
UpdateGUIO; 

pfFrameO; 
}//End  of  for 
}//End  of  step_upward 
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// 

//  Function:  jump 

//  Returns:  None 

//  Parameters:         Initial  position 

//  Summary:  Translate  the  whole  body  first  straight  upward, 

//  secondly  along  a  semi  circle  path,  then  straight  down 

//  and  straight  up  to  an  upright  position.  While  translating 

//  the  body  appropriate  joint  angles  are  applied. 

// 

void 

jump(float  X.float  Y,float  Z){ 

float  11  =  1.0f; 
float  12=1.  Of; 
float  cl_left,cl_right; 
float  thetal,theta2; 
float  kl_left,k2_left; 
float  kl_right,k2_right; 

float  x_left=    1.732f; 
float  y_left=    O.Of; 
float  x_right=  1.732f; 
float  y_right=  O.Of; 

for  (float  j  =  1   ;  j  < 101  ; j  +=  delta_t){ 
/*  Go  to  sleep  until  next  frame  time.  */ 
pfSync(); 
Shared->simTime  =  pfGefTimeO; 

/*     Main  Body     */ 

dcsO->setRot(90.0,  90.0 , 0); 

dcsO->setTrans(X+j *0. 8/1 00.0, Y,Z-j  *  1.5/100.0); 

/*     Head     */ 
dcs7->setTrans(0.0  ,  0.4  ,  0.0); 

/*     Torso     */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3  ,2.1,-0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3  ,2.1,0.005); 
dcs3->setRot(0,  10.0*j/100.0  ,  0.0); 
dcs6->setRot(0,  10.0*j/100.0  , 0.0); 

xjeft=  1.95-  1.0  *j/l  00.0; 
yjeft  =  0.0; 

cl_left=  ((x_left*x_left)  +  (y_left*y_left)  -  (11*11) 
-(12*12))/(2*11  *12); 

theta2  =  (l  *acos(cl_left)); 

kljeft  =  11  +  (12  *  cos(theta2)); 
k2_left  =  12  *  sin(theta2); 
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thetal  =  atan(y_left/x_left)  -  atan(k2_left/kl_left); 

dcs5->setRot(0  ,(57.3  *  thetal ),0  ); 
dcs4->setRot(0  ,(57.3  *  theta2) ,  0); 
dcs2->setRot(0  ,(57.3  *  thetal  ),0  ); 
dcsl->setRot(0  ,(57.3  *  theta2) ,  0); 

/*  Left  foot     */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0,  0  ,  180.0); 
dcs9->setTrans(0  ,-1.7,0); 

/*  Right  foot     */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0,  0  ,  180.0); 
dcs8->setTrans(0  ,-1.7,0); 

/*  Left  lowerleg     */ 
dcs4->sefTrans(0  ,-1.7,0); 

/*  Left  upperleg     */ 
dcs5->setTrans(0.6,  0,    0); 

/*  Right  lowerleg     */ 
dcsl->sefTrans(0  ,-1.7,0); 

/*  Right  upper  arm     */ 
dcsll->setScale(0.25f); 
dcsll->setTrans(-1.0  ,  1.7,0); 
dcsl  l->setRot(0.0,-j/5.0  ,0.0); 

/*  Right  lower  arm     */ 
dcsl2->setTrans(0.4  ,  -4.6,  0.0); 
dcsl2->setRot(0.0  ,-j/5.0,0.0); 

/*  Right  hand     */ 
dcsl3->setTrans(0.0  ,  -4.2,  0.0); 
dcsl3->setRot(0,  0,  180.0); 

/*  Left  upper  arm     */ 
dcsl4->setScale(0.25f); 
dcsl4->setTrans(1.6  ,  1.8,0); 
dcsl4->setRot(0.0  ,-j/5.0,0.0); 

/*  Left  lower  arm     */ 
dcsl5->setRot(0,-j/5.0,  180.0); 
dcsl5->setTrans(-0.5  ,  -4.6,  0.0); 

/*  Left  hand     */ 
dcsl6->setTrans(-0.2  ,  -4.6, 0.0); 

UpdateView(); 

UpdateGUK); 

pfFrame(); 
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for  (float  k  =  1  ;  k  <  101  ;  k  +=  5.0  *  delta_t){ 
/*  Go  to  sleep  until  next  frame  time.  */ 
pfSyncO; 
Shared->simTime  =  pfGetTimeQ; 

x_left  =  0.95  + 0.5  *k/100.0; 
yjeft  =  0.0; 

/*     Main  Body     */ 

dcs0->setRot(90.0  ,  90.0  +  15.0*k/100.0  ,  0); 

dcsO->setTrans(X+  100.0*0.8/100.0  + (xjeft*  1.7 
*  sin(15.0*k*DEG_TO_RAD/100.0)),  Y  , 
Z-  100.0  *  1 .5/100.0  +  0.95  *  k  /100.0  -  xjeft*  1 .7 
*(1.0  -  cos(15.0*k*DEG_TO_RAD/100.0))); 

/*     Head     */ 
dcs7->setTrans(0.0  ,  0.4  ,  0.0); 

/*     Torso     */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3  ,2.1,-0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3  ,2.1,0.005); 
dcs3->setRot(0,  10.0  , 0.0); 
dcs6->setRot(0,  10.0 , 0.0); 

cl_left=  ((x_left*x_left)  +  (y_left*y_left)  -  (11*11) 
-(12*12))/(2*11  *12); 

theta2  =  (l  *acos(cl_left)); 

kljeft  =  11  +  (12  *  cos(theta2)); 

k2_left  =  12  *  sin(theta2); 

thetal  =  atan(y_left/x_left)  -  atan(k2_left/kl_left); 

dcs5->setRot(0  ,(57.3  *  thetal )  ,0) 
dcs4->setRot(0  ,(57.3  *  theta2)  ,0) 
dcs2->setRot(0  ,(57.3  *  thetal)  ,0) 
dcsl->setRot(0  ,(57.3  *  theta2)  ,0) 

/*  Left  foot     */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0,  0  ,  180.0); 
dcs9->setTrans(0  ,-1.7,0); 

/*  Right  foot     */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0, 0,180.0); 
dcs8->setTrans(0  ,-1.7,0); 

/*  Left  lowerleg     */ 
dcs4->setTrans(0  ,-1.7,0); 

/*  Left  upperleg     */ 
dcs5->setTrans(0.6,  0,    0); 
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/*  Right  lowerleg     */ 
dcsl->setTrans(0  ,-1.7,0); 

/*  Right  upper  arm     */ 
dcsll->setScale(0.25f); 
dcsll->setTrans(-1.0,  1.7,0); 
dcsl  l->setRot(0.0,-j/5.0  ,0.0); 

/*  Right  lower  arm     */ 
dcsl2->setTrans(0.4 ,  -4.6,  0.0); 
dcsl2->setRot(0.0  ,-j/5.0,0.0); 

/*  Right  hand     */ 
dcsl3->setTrans(0.0 ,  -4.2,  0.0); 
dcsl3->setRot(0,  0,  180.0); 

/*  Left  upper  arm     */ 
dcsl4->setScale(0.25f); 
dcsl4->setTrans(1.6  ,1.8,0); 
dcsl4->setRot(0.0  ,-j/5.0,0.0); 

/*  Left  lower  arm     */ 
dcsl5->setRot(0,-j/5.0,  180.0); 
dcsl5->setTrans(-0.5  ,  -4.6, 0.0); 

/*  Left  hand     */ 
dcsl6->setTrans(-0.2  ,  -4.6,  0.0); 

UpdateView(); 

UpdateGUIO; 

pfFrame(); 

} 

for  (float  1=1   ;  1  <  101  ;  1  +=  delta_t){ 
/*  Go  to  sleep  until  next  frame  time.  */ 

pfSync(); 

Shared->simTime  =  pfGetTime(); 

x_left=1.45; 
yjeft  =  0.0; 

/*     Main  Body     */ 

dcsO->setRot(90.0  .  90.0+  360.0*1/100.0  ,  0); 

dcs0->setTrans(3.0  +  3.0*cos((  180.0  -  180.0*  1/100.0)*DEG_TO_RAD)+  X 
+  100.0*0.8/100.0  +(x_left*  1.7 

*  sin(15.0*100.0*DEG_TO_RAD/100.0)),Y  , 
3.0*sin((180.0-  180.0*  1/100.0)*DEG_TO_RAD) 
+  Z-  100.0  *  1.5/100.0+  0.95  *  100.0/100.0  -  xjeft 

*  1.7*(1.0-  cos(15.0*100.0*DEG_TO_RAD/100.0))); 

/*     Head     */ 
dcs7->setTrans(0.0  ,  0.4  ,  0.0); 
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/*     Torso     */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3  ,2.1,-0.005); 
dcs3->setScale(2.0f); 
dcs3->sefTrans(0.3  ,2.1,0.005); 
dcs3->setRot(0,  10.0,0.0); 
dcs6->setRot(0,  10.0,0.0); 

cl_left=  ((x_left*x_left)  +  (y_left*y_left)  -  (11*11) 
-  (12*12))/(2  *  11  *12); 

theta2  =  (l  *acos(cl_left)); 

kljeft  =  11  +  (12  *  cos(theta2)); 

k2_left  =  12  *  sin(theta2); 

thetal  =  atan(y_left/x_left)  -  atan(k2_left/kl_left); 


dcs5->setRot(0  ,(57.3  *  thetal), 0) 

dcs4->setRot(0  ,(57.3  *  theta2)  ,0) 

dcs2->setRot(0  ,(57.3  *  thetal)  ,0) 

dcsl->setRot(0  ,(57.3  *  theta2)  ,0) 

/*  Left  foot     */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0,  0  ,  180.0); 
dcs9->setTrans(0  ,-1.7,0); 

/*  Right  foot     */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0,  0  ,  180.0); 
dcs8->setTrans(0  ,-1.7,0); 

/*  Left  lowerleg     */ 
dcs4->setTrans(0  ,-1.7,0); 

/*  Left  upperleg     */ 
dcs5->setTraiis(0.6,  0,    0); 

/*  Right  lowerleg     */ 
dcsl->setTrans(0  ,-1.7,0); 

/*  Right  upper  arm     */ 
dcsll->setScale(0.25f); 
dcsll->setTrans(-1.0 ,  1.7,  0); 
dcsl  l->setRot(0.0,- 100.0/5.0  ,0.0); 

/*  Right  lower  arm     */ 
dcsl2->setTrans(0.4 ,  -4.6,  0.0); 
dcsl2->setRot(0.0  ,-100.0/5.0,0.0); 

/*  Right  hand     */ 
dcsl3->setTrans(0.0  ,  -4.2,  0.0); 
dcsl3->setRot(0,  0,  180.0); 
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/*  Left  upper  arm     */ 
dcsl4->setScale(0.25f); 
dcsl4->setTrans(1.6  ,1.8,0); 
dcs  1 4->setRot(0.0  ,- 1 00.0/5.0,0.0) ; 

/*  Left  lower  arm     */ 

dcs  1 5->setRot(0,- 1 00.0/5.0,  1 80.0); 

dcsl5->setTrans(-0.5  ,  -4.6,  0.0); 

/*  Left  hand     */ 
dcsl6->setTrans(-0.2 ,  -4.6, 0.0); 

UpdateView(); 

UpdateGUIO; 

pfFrameO; 

} 

for  (float  m  =  1   ;  m  <  101  ;  m  +=  3.0  *  delta_t){ 
/*  Go  to  sleep  until  next  frame  time.  */ 

pfSyncO; 

Shared->simTime  =  pfGetTime(); 

x_left=  1.45; 
y_left  =  0.0; 

/*     Main  Body     */ 

dcsO->setRot(90.0  ,  90.0+  360.0*100.0/100.0  ,  0); 

dcs0->setTrans(3.0  +  3.0*cos((180.0  -  180.0*  100.0/1 00.0)*DEG_TO_RAD)+X 
+  100.0*0.8/100.0 +  (x_left*  1.7 

*  sin(15.0*100.0*DEG_TO_RAD/100.0)),Y  , 
3.0*sin((180.0-  180.0*  100.0/100.0)*DEG_TO_RAD) 
+  Z-  100.0  *  1.5/100.0  +  0.95  *  100.0/100.0  -  xjeft 

*  1.7*(1.0-  cos(15.0*100.0*DEG_TO_RAD/100.0)) 
-  9.0 l*m/ 100.0); 

/*     Head     */ 
dcs7->setTrans(0.0  ,  0.4  ,  0.0); 

/*     Torso     */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3  ,2.1,-0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3  ,  2.1  ,  0.005); 
dcs3->setRot(0,  10.0,0.0); 
dcs6->setRot(0,  10.0  , 0.0); 

cl_left=  ((x_left*x_left)  +  (y_left*y_left)  -  (11*11) 
-  (12*12))/(2  *  11  *12); 

theta2  =  (1  *acos(cl_left)); 

kljeft  =  11  +  (12  *  cos(theta2)); 

k2_left  =  12  *  sin(theta2); 

thetal  =  atan(y_left/x_left)  -  atan(k2_left/kl_left); 

dcs5->setRot(0  ,(57.3  *  thetal)  ,0); 
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dcs4->setRot(0  ,(57.3  *  theta2)  ,0) 
dcs2->setRot(0  ,(57.3  *  thetal)  ,0) 
dcsl->setRot(0  ,(57.3  *  theta2)  ,0) 

/*  Left  foot     */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0,  0  ,  180.0); 
dcs9->setTrans(0  ,-1.7,0); 

/*  Right  foot     */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0,  0  ,  180.0); 
dcs8->setTrans(0  ,-1.7,0); 

/*  Left  lowerleg     */ 
dcs4->setTrans(0  ,-1.7,0); 

/*  Left  upperleg     */ 
dcs5->setTrans(0.6,  0,    0); 

/*  Right  lowerleg     */ 
dcsl->setTrans(0  ,-1.7,0); 

/*  Right  upper  arm     */ 
dcsll->setScale(0.25f); 
dcsll->setTrans(-1.0 ,  1.7,  0); 
dcsl  l->setRot(0.0,- 100.0/5.0  ,0.0); 

/*  Right  lower  arm     */ 
dcsl2->setTrans(0.4  ,  -4.6,  0.0); 
dcsl2->setRot(0.0  ,-100.0/5.0,0.0); 

/*  Right  hand     */ 
dcsl3->setTrans(0.0 ,  -4.2,  0.0); 
dcsl3->setRot(0,  0 ,  180.0); 

/*  Left  upper  arm     */ 
dcsl4->setScale(0.25f); 
dcsl4->setTrans(1.6  ,1.8,0); 
dcs  1 4->setRot(0.0  ,- 1 00.0/5.0,0.0); 

/*  Left  lower  arm     */ 

dcs  15->setRot(0,- 100.0/5.0,  180.0); 

dcsl5->setTrans(-0.5  ,  -4.6,  0.0); 

/*  Left  hand     */ 
dcsl6->setTrans(-0.2  ,  -4.6,  0.0); 


UpdateView(); 

UpdateGUIO; 

pfFrameO; 

} 

for  (float  n  =  1   ;  n  <  101  ;  n  +=  5.0  *  delta_t){ 

/*  Go  to  sleep  until  next  frame  time.  */ 

pfSyncQ; 
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Shared->simTime  =  pfGetTime(); 

x_left=  1 .45  +  0.5*n/100.0; 
yjeft  =  0.0; 

/*     Main  Body     */ 

dcs0->setRot(90.0  ,  90.0+  360.0*100.0/100.0  ,  0); 

dcs0->setTrans(3.0  +  3.0*cos((180.0  -  180.0*  100.0/1 00.0)  *DEG_TO_RAD)+X 
+  100.0*0.8/100.0 +  (1.45*  1.7 

*  sin(15.0*100.0*DEG_TO_RAD/100.0)),Y  , 
3.0*sin((  180.0-  180.0*  100.0/100.0)*DEG_TO_RAD) 
+  Z-  100.0*  1.5/100.0  +  0.95  *  100.0/100.0-  1.45 

*  1.7*(1.0  -  cos(15.0*100.0*DEG_TO_RAD/100.0)) 
-9.01*100.0/100.0+ n*  1.8/100.0); 

/*     Head     */ 
dcs7->setTrans(0.0  ,  0.4  ,  0.0); 

/*     Torso     */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3  ,  2.1  ,  -0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3  ,  2.1  ,  0.005); 
dcs3->setRot(0,  10.0 , 0.0); 
dcs6->setRot(0,  10.0,0.0); 

cl_left=  ((x_left*x_left)  +  (y_left*y_left)  -  (11*11) 
-(12*12))/(2*11  *12); 

theta2  =  (1  *acos(cl_left)); 

kljeft  =  11  +  (12  *  cos(theta2)); 

k2_left  =  12  *  sin(theta2); 

thetal  =  atan(y_left/x_left)  -  atan(k2_left/kl_left); 

dcs5->setRot(0  ,(57.3  *  thetal)  ,0) 

dcs4->setRot(0  ,(57.3  *  theta2)  ,0) 

dcs2->setRot(0  ,(57.3  *  thetal), 0) 

dcsl->setRot(0  ,(57.3  *  theta2)  ,0) 

/*  Left  foot     */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0,  0  ,  180.0); 
dcs9->setTrans(0  ,-1.7,0); 

/*  Right  foot     */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0,  0  ,  180.0); 
dcs8->setTrans(0  ,-1.7,0); 

/*  Left  lowerleg     */ 
dcs4->setTrans(0  ,-1.7,0); 

/*  Left-upperleg     */ 
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dcs5->setTrans(0.6,  0,    0); 

/*  Right  lowerleg     */ 
dcsl->setTrans(0  ,-1.7,0); 

/*  Right  upper  arm     */ 
dcsll->setScale(0.25f); 
dcsll->setTrans(-1.0,  1.7,0); 
dcsl  l->setRot(0.0,- 100.0/5.0  ,0.0); 

/*  Right  lower  arm     */ 
dcsl2->setTrans(0.4  ,  -4.6,  0.0); 
dcs  1 2->setRot(0.0  ,- 1 00.0/5.0,0.0); 

/*  Right  hand     */ 
dcsl3->setTrans(0.0 ,  -4.2,  0.0); 
dcsl3->setRot(0,  0,  180.0); 

/*  Left  upper  arm     */ 
dcsl4->setScale(0.25f); 
dcsl4->setTrans(1.6  ,1.8,0); 
dcsl4->setRot(0.0  ,-100.0/5.0,0.0); 

/*  Left  lower  arm     */ 

dcs  1 5->setRot(0,- 1 00.0/5.0,  1 80.0) ; 

dcsl5->setTrans(-0.5  ,  -4.6,  0.0); 

/*  Left  hand     */ 
dcsl6->setTrans(-0.2  ,  -4.6,  0.0); 

UpdateView(); 

UpdateGUIO; 

pfFrame(); 

} 

for  (float  o=l   ;  o  <  101  ;  o  +=  2.0  *  delta_t){ 
/*  Go  to  sleep  until  next  frame  time.  */ 

pfSync(); 

Shared->simTime  =  pfGetTime(); 

xjeft  =1.45  +  0.5  *  100.0/100.0; 
yjeft  =  0.0; 

/*     Main  Body     */ 

dcsO->setRot(90.0  ,  90.0+  360.0*100.0/100.0  ,  0); 

dcs0->setTrans(3.0  +  3.0*cos((  180.0  -  180.0*  100.0/100.0)*DEG_TO_RAD)+X 
+  100.0*0.8/100.0 +  (1.45*  1.7 

*  sin(15.0*100.0*DEG_TO_RAD/100.0)),Y  ,  3.0 
*sin((180.0-  180.0*  100.0/100.0)*DEG_TO_RAD) 

+  Z-  100.0*  1.5/100.0  +  0.95*  100.0/100.0-  1.45*  1.7 

*  (1.0  -  cos(l  5.0*  100.0*DEG_TO_RAD/1 00.0)) 
-9.01*  100.0/100.0 +  n*  1.8/100.0); 

/*     Head     */ 
dcs7->setTrans(0.0  ,  0.4  ,  0.0); 
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/*     Torso     */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3  ,2.1,-0.005); 
dcs3->setScale(2.00; 
dcs3->setTrans(0.3  ,  2.1  ,  0.005); 
dcs3->setRot(0,  10.0,0.0); 
dcs6->setRot(0,  10.0,0.0); 

cl_left=  ((xjeft*x_left)  +  (y_left*y_left)  -  (11*11) 
-  (12*12))/(2  *  11  *12); 

theta2  =  (l  *acos(cl_left)); 

kljeft  =  11  +  (12  *  cos(theta2)); 

k2_left  =  12  *  sin(theta2); 

thetal  =  atan(y_left/x_left)  -  atan(k2_left/kl_left); 


dcs5->setRot(0  ,(57.3  *  thetal)  ,0) 

dcs4->setRot(0  ,(57.3  *  theta2)  ,0) 

dcs2->setRot(0  ,(57.3  *  thetal)  ,0) 

dcsl->setRot(0  ,(57.3  *  theta2)  ,0) 

/*  Left  foot     */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0,  0  ,  180.0); 
dcs9->setTrans(0  ,-1.7,0); 

/*  Right  foot     */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0,  0  ,  180.0); 
dcs8->setTrans(0  ,-1.7,0); 

/*  Left  lowerleg     */ 
dcs4->setTrans(0  ,-1.7,0); 

/*  Left  upperleg     */ 
dcs5->setTrans(0.6,  0,    0); 

/*  Right  lowerleg     */ 
dcsl->setTrans(0  ,-1.7,0); 

/*  Right  upper  arm     */ 
dcsll->setScale(0.25f); 
dcsll->setTrans(-1.0,  1.7,0); 
dcsl  l->setRot(0.0,- 100.0/5.0  ,0.0); 

/*  Right  lower  arm     */ 
dcsl2->setTrans(0.4 ,  -4.6,  0.0); 
dcs  1 2->setRot(0.0  ,- 1 00.0/5.0,0.0); 

/*  Right  hand     */ 
dcsl3->setTrans(0.0  ,  -4.2,  0.0); 
dcsl3->setRot(0,  0,  180.0); 

/*  Left-upper  arm     */ 
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dcsl4->setScale(0.25f); 
dcsl4->setTrans(1.6  ,  1.8,0); 
dcsl4->setRot(0.0  ,-100.0/5.0,0.0); 

/*  Left  lower  arm     */ 

dcs  1 5->setRot(0,- 1 00.0/5.0,  1 80.0); 

dcsl5->setTrans(-0.5  ,  -4.6, 0.0); 

/*  Left  hand     */ 
dcsl6->setTrans(-0.2 ,  -4.6,  0.0); 

UpdateView(); 

UpdateGUIO; 

pfFrameO; 
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